<!DOCTYPE html>

<html>

  <head>
    <title>Underactuated Robotics</title>
    <meta name="Underactuated Robotics" content="text/html; charset=utf-8;" />

    <script type="text/javascript">
      function getParameterByName(name) {
        name = name.replace(/[\[]/, "\\[").replace(/[\]]/, "\\]");
        var regex = new RegExp("[\\?&]" + name + "=([^&#]*)"),
          results = regex.exec(location.search);
        return results == null ? "" : decodeURIComponent(results[1].replace(/\+/g, " "));
      }

      function chapterNumberToID(number) {
        if (number<0) { console.error("Bad chapter number"); }
        var chapters = document.getElementsByClassName("chapter");
        if ((number-1) < chapters.length) {
          return chapters[number-1].id;
        }
        console.error("Bad chapter number");
      }
      function chapterIDToNumber(id) {
        id = id.toLowerCase();
        var chapters = document.getElementsByClassName("chapter");
        for (j=0; j<chapters.length; j++) {
          if (chapters[j].id.toLowerCase() === id) {
            return j+1;
          }
        }
        console.error("Bad chapter ID");
        return -1;
      }

      function setPlatform(is_mac) {  // else display linux
        if (is_mac) {
          to_show = document.getElementsByClassName("mac");
          to_hide = document.getElementsByClassName("linux");
        } else {
          to_show = document.getElementsByClassName("linux");
          to_hide = document.getElementsByClassName("mac");
        }
	for (i=0; i<to_show.length; i++) {
          to_show[i].style.display = "inline";
	}
	for (i=0; i<to_hide.length; i++) {
	  to_hide[i].style.display = "none";
        }
      }

      function revealChapters() {
        // generate table of contents
        {

          var i,j;

          var toc = "<h1>Table of Contents</h1>\n<ul>\n<li><a href=#preface>Preface</a></li>\n";
          var chapters = document.getElementsByClassName("chapter");
          var app = document.getElementsByTagName("appendix");
          var start_appendix = chapters.length - app[0].getElementsByClassName("chapter").length;
          var current_part = chapters[0].parentNode;
          var part_number = 1;

          var list_of_drake_examples = "";

          // Set platform to linux unless I detect mac.
          setPlatform(navigator.appVersion.indexOf("Mac")!=1);

          for (i = 0; i < chapters.length; i++) {
            var titles=chapters[i].getElementsByTagName("h1");
            if (!chapters[i].id) {
              chapters[i].id = "chap"+(i+1);
            }
//            toc = toc + "<li><a href=#"+chapters[i].id+">";
            if (chapters[i].parentNode != current_part) {
              current_part = chapters[i].parentNode;
              part_number += 1;
              if (current_part.className=="part") {
                toc = toc + "<p style=\"margin-bottom: 0; text-decoration: underline; font-variant: small-caps;\"><b>" + current_part.getAttribute("title") + "</b></p>\n";
              }
            }

//            toc = toc + chapters[i].parentNode.tagName.toLowerCase();
            var chapter_link = "";
            if (chapters[i].parentNode.tagName.toLowerCase() == "appendix") {
              chapter_link =
                  "<li><a href=underactuated.html?chapter="+chapterNumberToID(i+1)+">";
              chapter_link += "Appendix " + String.fromCharCode(64 + i+1-start_appendix);
            } else {
              chapter_link =
                  "<li><a href=underactuated.html?chapter="+chapterNumberToID(i+1)+">";
              chapter_link += "Chapter " + (i+1);
            }
            chapter_link += ": " + titles[0].innerHTML + "</a>\n";
            toc += chapter_link;

            var sections = chapters[i].getElementsByTagName("section");
            var level=0;
            for (j=0; j<sections.length; j++) {
              if (sections[j].getAttribute("data-type")=="sect1") {
                while (level<1) { toc += "<ul>\n"; level += 1; }
                while (level>1) { toc += "</ul>\n"; level -= 1; }
              } else if (sections[j].getAttribute("data-type")=="sect2") {
                while (level<2) { toc += "<ul>\n"; level += 1; }
                while (level>2) { toc += "</ul>\n"; level -= 1; }
              } /*
              else if (sections[j].getAttribute("data-type")=="sect3") {
                while (level<3) { toc += "<ul>\n"; level += 1; }
                while (level>3) { toc += "</ul>\n"; level -= 1; }
              } */
              else { continue; }
              toc += "<li>" + sections[j].getElementsByTagName("h1")[0].innerHTML + "</li>\n";
            }
            while (level>0) { toc += "</ul>\n"; level -= 1; }

            toc += "</li>\n";

            // TODO: only do this work if I'm actually rendering the output (current_chapter id === drake)
            if (true) { // then I'm writing the drake example list, too
              var examples = chapters[i].getElementsByClassName("drake");
              if (examples.length>0) {
                list_of_drake_examples += chapter_link + "<ul>";
                for (j=0; j<examples.length; j++) {
                  var name = examples[j].getElementsByTagName("h1");
                  if (name.length>0) {
                    list_of_drake_examples += "<li>"+name[0].innerHTML+"</li>";
                  }
                }
                list_of_drake_examples += "</ul>\n";
              }

            }

          }

          toc = toc + "</ul>\n";
          document.getElementById("table_of_contents").innerHTML = toc;
        }

        var elibTags = document.getElementsByTagName('elib');
        // TODO(russt): identical references should reuse reference numbers.
        // TODO(russt): create actual bibliography section (similar to how I made the drake example list).
        for (i = 0; i < elibTags.length; i++) {
            var c = elibTags[i].innerHTML;
            elibTags[i].innerHTML = '[<a href="http://groups.csail.mit.edu/locomotion/elib.cgi?b=' + c + '">' + (i + 1) + '</a>]';
        }


        MathJax.Hub.Queue(["Typeset",MathJax.Hub,"mathjax_setup"]);

        var current_chapter = getParameterByName("chapter");

        if (current_chapter) {
          if (isNaN(current_chapter)) {
            current_chapter = chapterIDToNumber(current_chapter);
          } else {
            current_chapter = Number(current_chapter);
          }

          document.getElementById("preface").style.display = "none";
          document.getElementById("table_of_contents").style.display = "none";
          //document.getElementById("debug_output").innerHTML="displaying only chapter " + chapter + " of " + chapters.length;
          for (i = 0; i < chapters.length; i++) {
            if ((i+1) != current_chapter) {
              chapters[i].style.display = "none";
            } else {
              chapters[i].style.display = "inline";

              // load images/videos for this chapter only
              // TODO(russt): consider adding a lint test to make sure all
              // relevant elements are typed in as data-src instead of src.
              var images = chapters[i].getElementsByTagName("img");
              for (j=0; j<images.length; j++) {
                if (images[j].hasAttribute("data-src")) {
                  images[j].setAttribute("src",images[j].getAttribute("data-src"));
                }
              }
              // load images/videos for this chapter only
              var videos = chapters[i].getElementsByTagName("video");
              for (j=0; j<videos.length; j++) {
                if (videos[j].hasAttribute("data-src")) {
                  videos[j].setAttribute("src",videos[j].getAttribute("data-src"));
                }
                var extra_sources = videos[j].getElementsByTagName("source");
                for (k=0; k<extra_sources.length; k++) {
                  if (extra_sources[k].hasAttribute("data-src")) {
                    extra_sources[k].setAttribute("src", extra_sources[k].getAttribute("data-src"));
                  }
                }
              }
              var iframes = chapters[i].getElementsByTagName("iframe");
              for (j=0; j<iframes.length; j++) {
                if (iframes[j].hasAttribute("data-src")) {
                    iframes[j].setAttribute("src",iframes[j].getAttribute("data-src"));
                }
              }

              var codeTags = chapters[i].getElementsByTagName('pysrcinclude');
              for (j = 0; j < codeTags.length; j++) {
                  var file = 'src/' + codeTags[j].innerHTML;
                  var xhttp = new XMLHttpRequest();
                  xhttp.onreadystatechange = ( function (j) { return function() {
                      if (this.readyState == 4 && this.status == 200) {
                          var text = hljs.highlight('python',this.responseText,true)
                          codeTags[j].innerHTML =
                              '<div><pre><code class="python">'+text.value+'</code></pre></div>';
                      }
                  } })(j);
                  xhttp.overrideMimeType('text/plain');
                  xhttp.open('GET', file, true);
                  // note: only works for files in or below current directory http://jquery-howto.blogspot.com/2008/12/access-to-restricted-uri-denied-code.html
                  xhttp.send();
              }

              var pysrcTags = chapters[i].getElementsByTagName('pysrc');
              for (j = 0; j < pysrcTags.length; j++) {
                  var file = pysrcTags[j].innerHTML;
                  // TODO(russt): Consider checking that the file exists.
                  var tmp = '<p><pre style="margin-left:6px"><code>python <a href="src/'+ file + '">' + file + '</a>';
                  if (pysrcTags[j].hasAttribute("args")) {
                      tmp += ' '+ pysrcTags[j].getAttribute("args");
                  }
                  pysrcTags[j].innerHTML = tmp + '</code></pre></p>';
              }


//              document.getElementById("debug_output").innerHTML="got here";
              var display_num = i;
              if (i>=start_appendix) {
                display_num=i-start_appendix;
              }
              chapters[i].style.counterReset = "chapter " + display_num + " sect1 example_counter theorem algorithm figure";
              MathJax.Hub.Queue(["Typeset",MathJax.Hub,chapters[i]]);
              var nav = "\n<table style=\"width:100%;\"><tr style=\"width:100%\">";
              nav += "<td style=\"width:33%;text-align:left;\">";
              if (i>0) {
                nav+="<a href=underactuated.html?chapter=" +
                    chapterNumberToID(current_chapter-1) +
                    ">Previous chapter</a>";
              }
              nav += "</td><td style=\"width:33%;text-align:center;\"><a href=underactuated.html>Table of contents</a></td><td style=\"width:33%;text-align:right;\">";
              if ((i+1)<chapters.length) {
                nav += "<a href=underactuated.html?chapter=" +
                    chapterNumberToID(current_chapter+1) +
                    ">Next chapter</a>";
              }
              nav += "</td></tr></table>\n";
              chapters[i].innerHTML = nav + chapters[i].innerHTML + nav;
            }
          }
          if (chapters[current_chapter-1].id === "drake") {
            // then i'm in the drake appendix, so fill in that content

            // read drake_version.json and set binaries info
            var xhttp = new XMLHttpRequest();
            xhttp.onreadystatechange = ( function (j) { return function() {
              if (this.readyState == 4 && this.status == 200) {
                  var data = JSON.parse(this.responseText);

                  document.getElementById("drake-linux-binaries").innerHTML =
                      data.base_url + data.build + "/drake-" + data.version +
                      "-xenial.tar.gz";
                  document.getElementById("drake-mac-binaries").innerHTML =
                      data.base_url + data.build + "/drake-" + data.mac_version
                      + "-mac.tar.gz";
              }
            } })(j);
            xhttp.overrideMimeType('text/plain');
            xhttp.open('GET', 'drake_version.json', true);
            xhttp.send();

            document.getElementById("list_of_drake_examples").innerHTML =
                "<ul>" + list_of_drake_examples + "</ul>";

          }
        }

        var drakeTags = document.getElementsByTagName('drake');
        for (j = 0; j < drakeTags.length; j++) {
            drakeTags[j].innerHTML =
                '<a style="font-variant:small-caps; text-decoration:none;" href="http://drake.mit.edu">Drake</a>';
        }
      }
      </script>

    <script type="text/x-mathjax-config">
      MathJax.Hub.Config({
        skipStartupTypeset:true,
        extensions: ["tex2jax.js"],
        jax: ["input/TeX", "output/HTML-CSS"],
        TeX: { equationNumbers: {autoNumber: "AMS"}, noErrors: { disabled: true } },
        tex2jax: {
          inlineMath: [ ['$','$'], ["\\(","\\)"] ],
          displayMath: [ ['$$','$$'], ["\\[","\\]"] ],
          processEscapes: true
        },
        "HTML-CSS": { availableFonts: ["TeX"] }
      });
    </script>
    <script type="text/javascript"
      src="MathJax/MathJax.js?config=TeX-AMS_HTML">
    </script>

    <link rel="stylesheet" href="highlight/styles/default.css">
    <script src="highlight/highlight.pack.js"></script> <!-- http://highlightjs.readthedocs.io/en/latest/css-classes-reference.html#language-names-and-aliases -->
    <script>hljs.initHighlightingOnLoad();</script>

    <link rel="stylesheet" type="text/css" href="underactuated.css">

  </head>


<body data-type="book" class="book" id="htmlbook" onload="revealChapters();">
<section data-type="titlepage" class="titlepage">
  <header>
    <h1><a href="underactuated.html" style="text-decoration:none;">Underactuated Robotics</a></h1>
    <p data-type="subtitle">Algorithms for Walking, Running, Swimming, Flying, and Manipulation</p>
  	<p style="font-size: 18px;"><a href="http://people.csail.mit.edu/russt/">Russ Tedrake</a></p>
    <p style="font-size: 14px; text-align: right;">
      &copy; Russ Tedrake, 2018<br/>
      <a href="tocite.html">How to cite these notes</a><br/>
    </p>
  </header>
</section>
<div style="display:none" id="mathjax_setup"> <!-- definitions for mathjax -->
  \[
  \newcommand{\pd}[2]{\frac{\partial #1}{\partial #2}}
  \newcommand{\bc}{{\bf c}}
  \newcommand{\bg}{{\bf g}}
  \newcommand{\bh}{{\bf h}}
  \newcommand{\bq}{{\bf q}}
  \newcommand{\bv}{{\bf v}}
  \newcommand{\bx}{{\bf x}}
  \newcommand{\by}{{\bf y}}
  \newcommand{\bu}{{\bf u}}
  \newcommand{\bw}{{\bf w}}
  \newcommand{\bz}{{\bf z}}
  \newcommand{\bA}{{\bf A}}
  \newcommand{\bB}{{\bf B}}
  \newcommand{\bC}{{\bf C}}
  \newcommand{\bH}{{\bf H}}
  \newcommand{\bI}{{\bf I}}
  \newcommand{\bJ}{{\bf J}}
  \newcommand{\bK}{{\bf K}}
  \newcommand{\bM}{{\bf M}}
  \newcommand{\bQ}{{\bf Q}}
  \newcommand{\bR}{{\bf R}}
  \newcommand{\bT}{{\bf T}}
  \newcommand{\balpha}{{\bf \alpha}}
  \newcommand{\bbeta}{{\bf \beta}}
  \newcommand{\blambda}{{\bf \lambda}}
  \newcommand{\btau}{{\bf \tau}}
  \newcommand{\bphi}{{\bf \phi}}
  \newcommand{\bPhi}{{\bf \Phi}}
  \newcommand{\bpsi}{{\bf \psi}}
  \newcommand{\bPsi}{{\bf \Psi}}
  \newcommand{\avg}[1]{E\left[ #1 \right]}
  \newcommand{\subjto}{\textrm{subject to}}
  \newcommand{\minimize}{\operatorname{\textrm{minimize}}}
  \newcommand{\maximize}{\operatorname{\textrm{maximize}}}
  \newcommand{\find}{\operatorname{\textrm{find}}}
  \newcommand{\argmax}{\operatorname{\textrm{argmax}}}
  \newcommand{\argmin}{\operatorname{\textrm{argmin}}}
  \newcommand{\sgn}{\operatorname{\textrm{sgn}}}
  \newcommand{\trace}{\operatorname{\textrm{tr}}}
  \newcommand{\sos}{\text{ is SOS}}
  \]
</div>

<div id="debug_output"></div>

<p><b>Note:</b> These are working notes used for <a
href="http://underactuated.csail.mit.edu/Spring2018/">a course being taught
at MIT</a>. They will be updated throughout the Spring 2018
semester.</p>

<section id="table_of_contents"></section>


<section id="preface"><h1>Preface</h1>

  <p>This book is about building robots that move with speed, efficiency, and
  grace.  I believe that this can only be achieve through a tight coupling
  between mechanical design, passive dynamics, and nonlinear control synthesis.
  Therefore, these notes contain selected material from dynamical systems
  theory, as well as linear and nonlinear control.<p>

  <p>These notes also reflect a deep belief in computational algorithms playing
  an essential role in finding and optimizing solutions to complex dynamics and
  control problems.  Algorithms play an increasingly central role in modern
  control theory; these days even rigorous mathematicians consider finding
  convexity in a problem (therefore making it amenable to an efficient
  computational solution) almost tantamount to an analytical result.  Therefore,
  the notes necessarily also cover selected material from optimization theory,
  motion planning, and machine learning.</p>

  <p> Although the material in the book comes from many sources, the
  presentation is targeted very specifically at a handful of robotics problems.
  Concepts are introduced only when and if they can help progress the
  capabilities we are trying to develop.  Many of the disciplines that I am
  drawing from are traditionally very rigorous, to the point where the basic
  ideas can be hard to penetrate for someone that is new to the field. I've made
  a conscious effort in these notes to keep a very informal, conversational tone
  even when introducing these rigorous topics, and to reference the most
  powerful theorems but only to prove them when that proof would add particular
  insights without distracting from the mainstream presentation.  I hope that
  the result is a broad but reasonably self-contained and readable manuscript
  that will be of use to any enthusiastic roboticist.</p>

  <section><h1>Organization</h1>

    <p>The material in these notes is organized into a few main parts. "Model
    Systems" introduces a series of increasingly complex dynamical systems and
    overviews some of the relevant results from the literature for each system.
    "Nonlinear Planning and Control" introduces quite general computational
    algorithms for reasoning about those dynamical systems, with optimization
    theory playing a central role. Many of these algorithms treat the dynamical
    system as known and deterministic until the last chapters in this part which
    introduce stochasticity and robustness.  "Estimation and Learning" follows
    this up with techniques from statistics and machine learning which
    capitalize on this viewpoint to introduce additional algorithms which can
    operate with less assumptions on knowing the model or having perfect
    sensors.  The book closes with an "Appendix" that provides slightly more
    introduction (and references) for the main topics used in the course.</p>

    <p>The order of the chapters was chosen to make the book valuable as a
    reference.  When teaching the course, however, I take a spiral trajectory
    through the material, introducing robot dynamics and control problems one at
    a time, and introducing only the techniques that are required to solve that
    particular problem.</p>

    <todo>insert figure showing progression of problems here.  pendulum ->
    cp/acro -> walking ...  with chapter numbers associated.</todo>

  </section>

  <section><h1>Software</h1>

    <p> All of the examples and algorithms in this book, plus many more, are now
    available as a part of our open-source software project: <drake></drake>.
    <drake></drake> is a C++ project, but in this text we will use Drake's <a
    href="http://drake.mit.edu/python_bindings.html">Python bindings</a>.  I
    encourage super-users or readers who want to dig deeper to explore the C++
    code as well (and to contribute back). </p>

    <p>Please see the <a href="underactuated.html?chapter=drake">appendix</a>
    for specific instructions for using <drake></drake> along with these
    notes.</p>

  </section>

  <p style="text-align:right;"><a href="underactuated.html?chapter=intro">First chapter</a></p>

</section> <!-- end preface -->

<section data-type="chapter" class="chapter" id="intro">
<h1>Fully-actuated vs Underactuated Systems</h1>

  <p>Robots today move far too conservatively, and accomplish only a fraction of
  the tasks and achieve a fraction of the performance that they are mechanically
  capable of.  In many cases, we are still fundamentally limited by control
  technology which matured on rigid robotic arms in structured factory
  environments.  The study of underactuated robotics focuses on building control
  systems which use the natural dynamics of the machines in an attempt to
  achieve extraordinary performance in terms of speed, efficiency, or
  robustness.</p>

  <section data-type="sect1">
  <h1>Motivation</h1>

    <p>Let's start with some examples, and some videos.</p>

    <section data-type="sect2">
    <h1>Honda's ASIMO vs. passive dynamic walkers</h1>

      <p> The world of robotics changed when, in late 1996, Honda Motor Co.
      announced that they had been working for nearly 15 years (behind closed
      doors) on walking robot technology.  Their designs have continued to
      evolve, resulting in a humanoid robot they call ASIMO (Advanced Step in
      Innovative MObility).  For nearly 20 years, Honda's robots were widely
      considered to represent the state of the art in walking robots, although
      there are now many robots with designs and performance very similar to
      ASIMO's. We will dedicate effort to understanding a few of the details of
      ASIMO when we discuss algorithms for walking... for now I just want you to
      become familiar with the look and feel of ASIMO's movements [watch the
      asimo video below now]. </p>

      <figure>
        <video width="80%" controls>
          <source data-src="figures/walking_while_avoiding_people.ogg" type="video/ogg"/>
          <source data-src="http://world.honda.com/ASIMO/technology/2011/intelligence/video02/movie640w.mp4" type="video/mp4"/>
        </video><br/>
        <figcaption>Honda's ASIMO (from <a href="http://world.honda.com/ASIMO/video/">http://world.honda.com/ASIMO/video/</a>)</figcaption>
      </figure>

      <p> I hope that your first reaction is to be incredibly impressed with the
      quality and versatility of ASIMO's movements.  Now take a second look.
      Although the motions are very smooth, there is something a little
      unnatural about ASIMO's gait.  It feels a little like an astronaut
      encumbered by a heavy space suit.  In fact this is a reasonable analogy...
      ASIMO is walking a bit like somebody that is unfamiliar with his/her
      dynamics.  Its control system is using high-gain feedback, and therefore
      considerable joint torque, to cancel out the natural dynamics of the
      machine and strictly follow a desired trajectory. This control approach
      comes with a stiff penalty.  ASIMO uses roughly 20 times the energy
      (scaled) that a human uses to walk on the flat (measured by cost of
      transport)<elib>Collins05</elib>.  Also, control stabilization in this
      approach only works in a relatively small portion of the state space (when
      the stance foot is flat on the ground), so ASIMO can't move nearly as
      quickly as a human, and cannot walk on unmodelled or uneven terrain. </p>

      <figure>
        <p>
          <video width="80%" controls title="If your browser cannot play this video, then download it using the link below.">
            <source data-src="http://ruina.tam.cornell.edu/research/topics/locomotion_and_robotics/3d_passive_dynamic/from_angle.mpg" type="video/mp4"/>
            <source data-src="figures/passive_angle.ogg" type="video/ogg"/>
          </video><br/>
          <a href="http://ruina.tam.cornell.edu/research/topics/locomotion_and_robotics/3d_passive_dynamic/from_angle.mpg">Download the video</a>
        </p>
        <p>
          <video width="80%" controls title="If your browser cannot play this video, then download it using the link below.">
            <source data-src="http://ruina.tam.cornell.edu/research/topics/locomotion_and_robotics/3d_passive_dynamic/from_behind.mpg" type="video/mp4">
            <source data-src="figures/passive_behind.ogg" type="video/ogg"/>
          </video><br/>
          <a href="http://ruina.tam.cornell.edu/research/topics/locomotion_and_robotics/3d_passive_dynamic/from_behind.mpg">Download the video</a>
        </p>
        <figcaption>A 3D passive dynamic walker by Steve Collins and Andy Ruina<elib>Collins01</elib></figcaption>
      </figure>

      <p> For contrast, let's now consider a very different type of walking
      robot, called a passive dynamic walker (PDW).  This "robot" has no motors,
      no controllers, no computer, but is still capable of walking stably down a
      small ramp, powered only by gravity [watch videos above now].  Most people
      will agree that the passive gait of this machine is more natural than
      ASIMO's; it is certainly more efficient.  Passive walking machines have a
      long history - there are patents for passively walking toys dating back to
      the mid 1800's.  We will discuss, in detail, what people know about the
      dynamics of these machines and what has been accomplished experimentally.
      This most impressive passive dynamic walker to date was built by Steve
      Collins in Andy Ruina's lab at Cornell<elib>Collins01</elib>. </p>

      <p> Passive walkers demonstrate that the high-gain, dynamics-cancelling
      feedback approach taken on ASIMO is not a necessary one.  In fact, the
      dynamics of walking is beautiful, and should be exploited - not cancelled
      out. </p>

      <p> The world is just starting to see what this vision could look like.
      This video from Boston Dynamics is one of my favorites of all time:</p>

      <figure>
      <iframe width="560" height="315"
              data-src="https://www.youtube.com/embed/fRj34o4hN4I?rel=0"
              frameborder="0" allow="autoplay; encrypted-media" allowfullscreen></iframe>
      <figcaption>Boston Dynamics' Atlas robot does a backflip.</figcaption>
      </figure>

      <p> This result is a marvel of engineering (the mechanical design alone is
      amazing...).  In this class, we'll teach you the computational tools to
      required to make robots perform this way.  We'll also try to reason about
      how robust these types of maneuvers are and can be.  Don't worry, if you
      do not have a super lightweight, super capable, and super durable
      humanoid, then a simulation will be provided for you.  </p> </section>

    <section data-type="sect2"><h1>Birds vs. modern aircraft</h1>

      <p> The story is surprisingly similar in a very different type of machine.
      Modern airplanes are extremely effective for steady-level flight in still
      air. Propellers produce thrust very efficiently, and today's cambered
      airfoils are highly optimized for speed and/or efficiency. It would be
      easy to convince yourself that we have nothing left to learn from birds.
      But, like ASIMO, these machines are mostly confined to a very
      conservative, low angle-of-attack flight regime where the aerodynamics on
      the wing are well understood.  Birds routinely execute maneuvers outside
      of this flight envelope (for instance, when they are landing on a perch),
      and are considerably more effective than our best aircraft at exploiting
      energy (eg, wind) in the air.  </p>

      <p> As a consequence, birds are extremely efficient flying machines; some
      are capable of migrating thousands of kilometers with incredibly small
      fuel supplies.  The wandering albatross can fly for hours, or even days,
      without flapping its wings - these birds exploit the shear layer formed by
      the wind over the ocean surface in a technique called dynamic soaring.
      Remarkably, the metabolic cost of flying for these birds is
      indistinguishable from the baseline metabolic cost<elib>Arnould96</elib>,
      suggesting that they can travel incredible distances (upwind or downwind)
      powered almost completely by gradients in the wind.  Other birds achieve
      efficiency through similarly rich interactions with the air - including
      formation flying, thermal soaring, and ridge soaring.  Small birds and
      large insects, such as butterflies and locusts, use "gust soaring" to
      migrate hundreds or even thousands of kilometers carried primarily by the
      wind. </p>

      <p> Birds are also incredibly maneuverable.  The roll rate of a highly
      acrobatic aircraft (e.g, the A-4 Skyhawk) is approximately 720
      deg/sec<elib>Shyy08</elib>; a barn swallow has a roll rate in excess of
      5000   deg/sec<elib>Shyy08</elib>. Bats can be flying at full-speed in one
      direction, and completely reverse direction while maintaining forward
      speed, all in just over 2 wing-beats and in a distance less than half the
      wingspan<elib>Tian06</elib>.  Although quantitative flow visualization
      data from maneuvering flight is scarce, a dominant theory is that the
      ability of these animals to produce sudden, large forces for
      maneuverability can be attributed to unsteady aerodynamics, e.g., the
      animal creates a large suction vortex to rapidly change
      direction<elib>Triantafyllou95</elib>.  These astonishing capabilities are
      called upon routinely in maneuvers like flared perching, prey-catching,
      and high speed flying through forests and caves.  Even at high speeds and
      high turn rates, these animals are capable of incredible agility - bats
      sometimes capture prey on their wings, Peregrine falcons can pull 25 G's
      out of a 240 mph dive to catch a sparrow in
      mid-flight<elib>Tucker98</elib>, and even the small birds outside our
      building can be seen diving through a chain-link fence to grab a bite of
      food. </p>

      <p> Although many impressive statistics about avian flight have been
      recorded, our understanding is partially limited by experimental
      accessibility - it is quite difficult to carefully measure birds (and the
      surrounding airflow) during their most impressive maneuvers without
      disturbing them.  The dynamics of a swimming fish are closely related, and
      can be more convenient to study. Dolphins have been known to swim
      gracefully through the waves alongside ships moving at 20
      knots<elib>Triantafyllou95</elib>.  Smaller fish, such as the bluegill
      sunfish, are known to possess an escape response in which they propel
      themselves to full speed from rest in less than a body length; flow
      visualizations indeed confirm that this is accomplished by creating a
      large suction vortex along the side of the body<elib>Tytell08</elib> -
      similar to how bats change direction in less than a body length. There are
      even observations of a dead fish swimming upstream by pulling energy out
      of the wake of a cylinder; this passive propulsion is presumably part of
      the technique used by rainbow trout to swim upstream at mating
      season<elib>Beal06</elib>. </p>

    </section>

    <section data-type="sect2"><h1>Manipulation</h1>

      <p>Despite a long history of success in industrial applications, and the
      huge potential for consumer applications, we still don't have robot arms
      that can perform any meaningful tasks in the home.  Admittedly, the
      perception problem (using sensors to detect/localize objects and
      understand the scene) for home robotics is incredibly difficult. But even
      if we were given a perfect perception system, our robots are still a long
      way from performing basic object manipulation tasks with the dexterity and
      versatility of a human. </p>

      <p>Most robots that perform object manipulation today use a stereotypical
      pipeline.  First, we enumerate a handful of contact locations on the hand
      (these points, and only these points, are allowed to contact the world).
      Then, given a localized object in the environment, we plan a
      collision-free trajectory for the arm that will move the hand into a
      "pre-grasp" location. At this point the robot closes it's eyes
      (figuratively) and closes the hand, hoping that the pre-grasp location was
      good enough that the object will be successfully grasped using e.g. only
      current feedback in the fingers to know when to stop closing.
      "Underactuated hands" make this approach more successful, but the entire
      approach really only works well for enveloping grasps. </p>

      <p>The enveloping grasps approach may actually be sufficient for a number
      of simple pick-and-place tasks, but it is a very poor representation of
      how humans do manipulation.  When humans manipulate objects, the contact
      interactions with the object and the world are very rich -- we often use
      pieces of the environment as fixtures to reduce uncertainty, we commonly
      <i>exploit</i> slipping behaviors (e.g. for picking things up, or
      reorienting it in the hand), and our brains don't throw NaNs if we use the
      entire surface of our arms to e.g. manipulate a large object. </p>

      <todo>Add videos.  Salami pick and place?  Rick's box turning?  Alex's
      disney bubbles?</todo>

      <p>By the way, in most cases, if the robots fail to make contact at the
      anticipated contact times/locations, <a
      href="https://youtu.be/g0TaYhjpOfo?t=48s">bad things can happen</a>. The
      results are hilarious and depressing at the same time.  (Let's fix
      that!)</p>

    </section>

    <section data-type="sect2"><h1>The common theme</h1>

      <p> Classical control techniques for robotics are based on the idea that
      feedback can be used to override the dynamics of our machines.  These
      examples suggest that to achieve outstanding dynamic performance
      (efficiency, agility, and robustness) from our robots, we need to
      understand how to design control systems which take advantage of the
      dynamics, not cancel them out.  That is the topic of this course. </p>

      <p> Surprisingly, there are relatively few formal control ideas that
      consider "exploiting" the dynamics.  In order to convince a control
      theorist to consider the dynamics (efficiency arguments are not enough),
      you have to do something drastic, like taking away his control authority -
      remove a motor, or enforce a torque-limit.  These issues have created a
      formal class of systems, the underactuated systems, for which people have
      begun to more carefully consider the dynamics of their machines in the
      context of control. </p>

    </section>

  </section>

  <section data-type="sect1"><h1>Definitions</h1>

    <p> According to Newton, the dynamics of mechanical systems are second order
    ($F = ma$).  Their state is given by a vector of positions, $\bq$ (also
    known as the configuration vector), and a vector of velocities, $\dot{\bq}$,
    and (possibly) time. The general form for a second-order controllable
    dynamical system is: $$\ddot{\bq} = {\bf f}(\bq,\dot{\bq},\bu,t),$$ where
    $\bu$ is the control vector.  As we will see, the dynamics for many of the
    robots that we care about turn out to be affine in commanded torque, so
    let's consider a slightly constrained form: \begin{equation}\ddot{\bq} =
    {\bf f}_1(\bq,\dot{\bq},t) + {\bf f}_2(\bq,\dot{\bq},t)\bu
    \label{eq:f1_plus_f2}.\end{equation} </p>

    <div data-type="definition"><h1>Fully-Actuated</h1> A control system
    described by equation \ref{eq:f1_plus_f2} is fully-actuated in state
    $(\bq,\dot{\bq})$ at time $t$ if it is able to command any instantaneous
    acceleration in $\bq$: \begin{equation} \textrm{rank}\left[{\bf f}_2
    (\bq,\dot{\bq},t)\right] = \dim\left[\bq\right].\end{equation} </div>

    <div data-type="definition"><h1>Underactuated</h1> A control system
    described by equation \ref{eq:f1_plus_f2} is underactuated in state
    $(\bq,\dot{\bq})$ at time $t$ if it is not able to command an arbitrary
    instantaneous acceleration in $\bq$: \begin{equation}
    \textrm{rank}\left[{\bf f}_2(\bq,\dot{\bq},t)\right] < \dim\left[\bq\right].
    \label{eq:underactuated_def}\end{equation} </div>

    <p>Notice that whether or not a control system is underactuated may depend
    on the state of the system or even on time, although for most systems
    (including all of the systems in this book) underactuation is a global
    property of the system.  We will refer to a system as underactuated if it is
    underactuated in <i>all</i> states and times.  In practice, we often refer
    informally to systems as fully actuated as long as they are fully actuated
    in <i>most</i> states (e.g., a "fully-actuated" system might still have
    joint limits or lose rank at a kinematic singularity).  Admittedly, this
    permits the existence of a gray area, where we would prefer to avoid
    describing the <i>system</i> as either fully- or underactuated (we should
    instead only describe its states); but if you find a physical system that is
    sort of "half and half", then do let me know!</p>

    <div data-type="example" class="drake"><h1>Robot Manipulators</h1>

      <figure>
      <img style="width:250px;" data-src="figures/simple_double_pend.svg"/>
      <todo>make this image spring to life with a python movie<</todo>
      <figcaption>Simple double pendulum</figcaption>
      </figure>

      <p> Consider the simple robot manipulator illustrated above.  As described
      in the <a href="underactuated.html?chapter=multibody">Appendix</a>, the
      equations of motion for this system are quite simple to derive, and take the
      form of the standard "manipulator equations": $${\bf M}(\bq)\ddot\bq +
      \bC(\bq,\dot\bq)\dot\bq = \btau_g(\bq) + {\bf B}\bu.$$ It is well known that
      the inertia matrix, ${\bf M}(\bq)$ is (always) uniformly symmetric and
      positive definite, and is therefore invertible. Putting the system into the
      form of equation \ref{eq:f1_plus_f2} yields: \begin{align*}\ddot{\bq} =&
      {\bf M}^{-1}(\bq)\left[ \btau_g(\bq) + \bB\bu - \bC(\bq,\dot\bq)\dot\bq
      \right].\end{align*} Because ${\bf M}^{-1}(\bq)$ is always full rank, we
      find that a system described by the manipulator equations is fully-actuated
      if and only if $\bB$ is full row rank. For this particular example,
      $\bq = [\theta_1,\theta_2]^T$ and $\bu = [\tau_1,\tau_2]^T$ (motor torques
      at the joints), and $\bB = {\bf I}_{2 \times 2}$.  The system is fully
      actuated.</p>

      <todo>make drake elements expandable/collapsible.  nice example here:
      http://quhno.internetstrahlen.de/myopera/csstests/collapsible-paragraph.html</todo>

      <div data-type="drake"><h1>Python Example</h1>

      <p> I personally learn best when I can experiment and get some physical
      intuition.  The <a href="http://drake.mit.edu">companion software for the
      course</a> should make it easy for you to see this system in action.  To try
      it, make sure you've followed the installation instructions in the <a
      href="?chapter=drake">Appendix</a>, then open Python and try the following
      lines in your Python console:</p>

      <pysrcinclude>double_pendulum/simulator.py</pysrcinclude>

      <p>It's worth taking a peek at the <a
      href="src/double_pendulum/double_pendulum.urdf">file that describes the
      robot</a>. URDF and SDF are two of the standard formats, and they can be
      used to describe even very complicated robots (like the Boston Dynamics
      humanoid).</p>

      <p>We can also use <drake></drake> to evaluate the manipulator equations
      .  <drake></drake> actually has a symbolic dynamics engine, but for now
      we will just evaluate the manipulator equations for a particular robot
      (with numerical values assigned for mass, link lengths, etc) and for a
      particular state of the robot:</p>

      <pysrcinclude>double_pendulum/manipulator.py</pysrcinclude>

      <todo>Implement the symbolic version</todo>

      </div>
    </div>

    <p> While the basic double pendulum is fully actuated, imagine the somewhat
    bizarre case that we have a motor to provide torque at the elbow, but no
    motor at the shoulder.  In this case, we have $\bu = \tau_2$, and $\bB(\bq)
    = [0,1]^T$.  This system is clearly underactuated.  While it may sound like
    a contrived example, it turns out that it is almost exactly the dynamics we
    will use to study as our simplest model of walking later in the class. </p>

    </div>

    <p>The matrix ${\bf f}_2$ in equation \ref{eq:f1_plus_f2} always has
    dim$[\bq]$ rows, and dim$[\bu]$ columns. Therefore, as in the example, one
    of the most common cases for underactuation, which trivially implies that
    ${\bf f}_2$ is not full row rank, is dim$[\bu] < $ dim$[\bq]$.  This is the
    case when a robot has joints with no motors. But this is not the only case.
    The human body, for instance, has an incredible number of actuators
    (muscles), and in many cases has multiple muscles per joint; despite having
    more actuators than position variables, when I jump through the air, there
    is no combination of muscle inputs that can change the ballistic trajectory
    of my center of mass (barring aerodynamic effects).  My control system is
    underactuated.</p>

    <p>For completeness, let's generalize the definition of underactuation to
    systems beyond the second-order control affine systems.</p>

    <div data-type="definition"> <h1>Underactuated Control Differential
    Equations</h1> An $n$th-order control differential equation control
    described by the equations \begin{equation} \frac{d^n\bq}{dt^n} = f(\bq,
    ..., \frac{d^{n-1} \bq}{dt^{n-1}}, t, {\bf u}) \end{equation} is fully
    actuated in state ${\bf x} = (\bq, ..., \frac{d^{n-1} \bq}{dt^{n-1}})$ and
    time $t$ if the resulting map ${\bf f}$ is surjective: for every
    $\frac{d^n\bq}{dt^n} $ there exists a ${\bf u}$ which produces the desired
    response.  Otherwise it is underactuated. </div>

    <p>It is easy to see that equation \ref{eq:underactuated_def} is a
    sufficient condition for underactuation.  This definition can also be
    extended to discrete-time systems and/or differential inclusions.</p>

    <p>A quick note about notation.  When describing the dynamics of rigid-body
    systems in this class, I will use $\bq$ for configurations (positions),
    $\dot{\bq}$ for velocities, and use $\bx$ for the full state ($\bx =
    [\bq^T,\dot{\bq}^T]^T$).  There is an important limitation to this
    convention (3D angular velocity should not be represented as the derivative
    of 3D pose) described in the Appendix<!-- ch:robot_dynamics -->, but it will
    keep the notes cleaner. Unless otherwise noted, vectors are always treated
    as column vectors. Vectors and matrices are bold (scalars are not). </p>

  </section>

  <section data-type="sect1"> <h1>Feedback Linearization</h1>

    <p> Fully-actuated systems are dramatically easier to control than
    underactuated systems.  The key observation is that, for fully-actuated
    systems with known dynamics (e.g., ${\bf f}_1$ and ${\bf f}_2$ are known for
    a second-order control-affine system), it is possible to use feedback to
    effectively change a nonlinear control problem into a linear control
    problem.  The field of linear control is incredibly advanced, and there are
    many well-known solutions for controlling linear systems. </p>

    <p>The trick is called feedback linearization.  When ${\bf f}_2$ is full row
    rank, it is invertible<sidenote>If ${\bf f}_2$ is not square, for instance
    you have multiple actuators per joint, then this inverse may not be
    unique.</sidenote>. Consider the nonlinear feedback law: $$\bu = {\bf
    \pi}(\bq,\dot\bq,t) = {\bf f}_2^{-1}(\bq,\dot\bq,t) \left[ \bu' - {\bf
    f}_1(\bq,\dot\bq,t) \right],$$ where $\bu'$ is the new control input (an
    input to your controller). Applying this feedback controller to equation
    \ref{eq:f1_plus_f2} results in the linear, decoupled, second-order system:
    $$\ddot{\bq} = \bu'.$$ In other words, if ${\bf f}_1$ and ${\bf f}_2$ are
    known and ${\bf f}_2$ is invertible, then we say that the system is
    "feedback equivalent" to $\ddot{\bq} = \bu'$.  There are a number of strong
    results which generalize this idea to the case where ${\bf f}_1$ and ${\bf
    f}_2$ are estimated, rather than known (e.g, <elib>Slotine90</elib>).</p>

    <div data-type="example" class="drake"><h1>Feedback-Cancellation Double
    Pendulum</h1>

    <p> Let's say that we would like our simple double pendulum to act like a
    simple single pendulum (with damping), whose dynamics are given by:
    \begin{align*} \ddot \theta_1 &= -\frac{g}{l}\sin\theta_1 -b\dot\theta_1 \\
    \ddot\theta_2 &= 0. \end{align*} This is easily achieved using
    <sidenote>Note that our chosen dynamics do not actually stabilize $\theta_2$
    - this detail was left out for clarity, but would be necessary for any real
    implementation.</sidenote> $$\bu = \bB^{-1}\left[ \bC\dot{\bq} - \btau_g +
    {\bf M}\begin{bmatrix} -\frac{g}{l}s_1 - b\dot{q}_1 \\ 0 \end{bmatrix}
    \right].$$ </p>

    <p> Since we are embedding a nonlinear dynamics (not a linear one), we refer
    to this as "feedback cancellation", or "dynamic inversion".  This idea can,
    and does, make control look easy - for the special case of a fully-actuated
    deterministic system with known dynamics.  For example, it would have been
    just as easy for me to invert gravity. Observe that the control derivations
    here would not have been any more difficult if the robot had 100 joints.
    </p>

    <div data-type="drake"><h1>Python Example</h1> You can run these examples
    using:

    <pysrc args="--gravity=9.8">double_pendulum/as_single_pendulum.py</pysrc>

    <pysrc args="--gravity=-9.8">double_pendulum/as_single_pendulum.py</pysrc>

    <p>As always, make soure you take a moment to read through the source
    code (linked above).</p>

    </div>

    </div> <!--end example-->

    <p> The underactuated systems are not feedback linearizable.  Therefore,
    unlike fully-actuated systems, the control designer has no choice but to
    reason about the nonlinear dynamics of the plant in the control design.
    This dramatically complicates feedback controller design. </p>

    

  </section>

  <section data-type="sect1"><h1>Input and State Constraints</h1>

    <p> Although the dynamic constraints due to missing actuators certainly
    embody the spirit of this course, many of the systems we care about could be
    subject to other dynamic constraints as well.  For example, the actuators on
    our machines may only be mechanically capable of producing some limited
    amount of torque, or there may be a physical obstacle in the free space with
    which we cannot permit our robot to come into contact with. </p>

    <div data-type="definition"><h1>Input and State Constraints</h1> A dynamical
    system described by $\dot{\bx} = {\bf f}(\bx,\bu,t)$ may be subject to one
    or more constraints described by $\bphi(\bx,\bu,t)\ge0$. </div>

    <p>In practice it can be useful to separate out constraints which depend
    only on the input, e.g. $\phi(\bu)\ge0$, such as actuator limits, as they
    can often be easier to handle than state constraints.  An obstacle in the
    environment might manifest itself as one or more constraints that depend
    only on position, e.g. $\phi(\bq)\ge0$.</p>

    <p>By our generalized definition of underactuation, we can see that input
    constraints can certainly cause a system to be underactuated.  State (only)
    constraints are more subtle -- in general these actually reduce the
    dimensionality of the state space, therefore requiring less dimensions of
    actuation to achieve "full" control, but we only reap the benefits if we are
    able to perform the control design in the "minimal coordinates" (which is
    often difficult). </p>

    <div data-type="example"><h1>Input limits</h1> Consider the constrained
    second-order linear system \[ \ddot{x} = u, \quad |u| \le 1. \] By our
    definition, this system is underactuated.  For example, there is no $u$
    which can produce the acceleration $\ddot{x} = 2$. </div>

    <p>Input and state constraints can complicate control design in similar ways
    to having an insufficient number of actuators, (i.e., further limiting the
    set of the feasible trajectories), and often require similar tools to find a
    control solution.</p>

    <section data-type="sect2"><h1>Nonholonomic constraints</h1>

      <p> You might have heard of the term "nonholonomic system" (see e.g.
      <elib>Bloch03</elib>), and be thinking about how nonholonomy relates to
      underactuation.  Briefly, a nonholonomic constraint is a constraint of the
      form $\phi(\bq, {\bf \dot{q}}, t) = 0$, which cannot be integrated into a
      constraint of the form $\phi(\bq, t) = 0$ (a holonomic constraint). A
      nonholonomic constraint does not restrain the possible configurations of
      the system, but rather the manner in which those configurations can be
      reached. While a holonomic constraint reduces the number of degrees of
      freedom of a system by one, a nonholonomic constraint does not.  An
      automobile or traditional wheeled robot provides a canonical example:</p>

      <div data-type="example"><h1>Wheeled robot</h1> Consider a simple model of
      a wheeled robot whose configuration is described by its Cartesian position
      $x,y$ and its orientation, $\theta$, so $\bq = \begin{bmatrix} x, y,
      \theta \end{bmatrix}^T$.  The system is subject to a differential
      constraint that prevents side-slip, \begin{gather*} \dot{x} = v \cos\theta
      \\ \dot{y} = v \sin\theta \\ v = \sqrt{\dot{x}^2 + \dot{y}^2}
      \end{gather*} or equivalently, \[\dot{y} \cos \theta - \dot x \sin \theta
      = 0.\] This constraint cannot be integrated into a constraint on
      configuration&mdash;the car can get to any configuration $(x,y,\theta)$,
      it just can't move directly sideways&mdash;so this is a nonholonomic
      constraint. </div>

      <p>Contrast the wheeled robot example with a robot on train tracks.  The
      train tracks correspond to a holonomic constraint: the track constraint
      can be written directly in terms of the configuration $\bq$ of the system,
      without using the velocity ${\bf \dot{q}}$. Even though the track
      constraint could also be written as a differential constraint on the
      velocity, it would be possible to integrate this constraint to obtain a
      constraint on configuration. The track restrains the possible
      configurations of the system.</p>

      <p> A nonholonomic constraint like the no-side-slip constraint on the
      wheeled vehicle certainly results in an underactuated system.  The
      converse is not necessarily true&mdash;the double pendulum system which is
      missing an actuator is underactuated but would not typically be called a
      nonholonomic system.  Note that the Lagrangian equations of motion are a
      constraint of the form \[\bphi(\bq,{\bf \dot{q}},{\bf \ddot{q}},t) =  0,\]
      so do not qualify as a nonholonomic constraint.</p>

    </section>

  </section>

  <section data-type="sect1"><h1>Underactuated robotics</h1>

    <p> The control of underactuated systems is an open and interesting problem
    in controls.  Although there are a number of special cases where
    underactuated systems have been controlled, there are relatively few general
    principles.  Now here's the rub... most of the interesting problems in
    robotics are underactuated: </p>

    <ul>

      <li> Legged robots are underactuated.  Consider a legged machine with $N$
      internal joints and $N$ actuators.  If the robot is not bolted to the
      ground, then the degrees of freedom of the system include both the
      internal joints and the six degrees of freedom which define the position
      and orientation of the robot in space.  Since $\bu \in \Re^N$ and $\bq \in
      \Re^{N+6}$, equation \ref{eq:underactuated_def} is satisfied.</li>

      <li> (Most) Swimming and flying robots are underactuated.  The story is
      the same here as for legged machines.  Each control surface adds one
      actuator and one DOF.  And this is already a simplification, as the true
      state of the system should really include the (infinite-dimensional) state
      of the flow.</li>

      <li> Robot manipulation is (often) underactuated.  Consider a
      fully-actuated robotic arm.  When this arm is manipulating an object with
      degrees of freedom (even a brick has six), it can become underactuated. If
      force closure is achieved, and maintained, then we can think of the system
      as fully-actuated, because the degrees of freedom of the object are
      constrained to match the degrees of freedom of the hand.  That is, of
      course, unless the manipulated object has extra DOFs (for example, any
      object that is deformable).</li>

    </ul>

    <p> Even control systems for fully-actuated and otherwise unconstrained
    systems can be improved using the lessons from underactuated systems,
    particularly if there is a need to increase the efficiency of their motions
    or reduce the complexity of their designs.</p>

  </section>

  <section data-type="sect1"><h1>Goals for the course</h1>

    <p> This course is based on the observation that there are new computational
    tools from optimization theory, control theory, motion planning, and even
    machine learning which can be used to design feedback control for
    underactuated systems.  The goal of this class is to develop these tools in
    order to design robots that are more dynamic and more agile than the current
    state-of-the-art.</p>

    <p> The target audience for the class includes both computer science and
    mechanical/aero students pursuing research in robotics.  Although I assume a
    comfort with linear algebra, ODEs, and Python, the course notes aim to
    provide most of the material and references required for the course. </p>

  </section>

</section> <!-- end chapter (intro)-->


<div class="part" title="Model Systems">


<section data-type="chapter" class="chapter" id="pend">
<h1>The Simple Pendulum</h1>

  <section data-type="sect1"><h1>Introduction</h1>

    <p> Our goals for this chapter are modest: we'd like to understand the
    dynamics of a pendulum.</p>

    <p>Why a pendulum?  In part, because the dynamics of a majority of our
    multi-link robotics manipulators are simply the dynamics of a large number
    of coupled pendula.  Also, the dynamics of a single pendulum are rich enough
    to introduce most of the concepts from nonlinear dynamics that we will use
    in this text, but tractable enough for us to (mostly) understand in the next
    few pages. </p>

    <figure> <img width="30%" data-src="figures/simple_pend.svg"/>
    <figcaption>The simple pendulum</figcaption> </figure>

    <p> The Lagrangian derivation of the equations of motion (as described in
    the appendix) of the simple pendulum yields: \begin{equation*} m l^2
    \ddot\theta(t) + mgl\sin{\theta(t)} = Q. \end{equation*} We'll consider the
    case where the generalized force, $Q$, models a damping torque (from
    friction) plus a control torque input, $u(t)$: $$Q = -b\dot\theta(t) +
    u(t).$$ </p>

  </section>

  <section data-type="sect1" class="drake"><h1>Nonlinear dynamics with a
    constant
    torque</h1>

    <p> Let us first consider the dynamics of the pendulum if it is driven in a
    particular simple way: a torque which does not vary with time:
    \begin{equation} ml^2 \ddot\theta + b\dot\theta + mgl \sin\theta = u_0.
      \end{equation} </p>.


    <div data-type="example" class="drake"><h1>Simple Pendulum in Python</h1>

      <p>You can experiment with this system in <drake></drake> using</p>

      <pysrc>pendulum/torque_slider_demo.py</pysrc>

      <p>(use the slider at the top to adjust the torque input).</p>

    </div>

    <p> These are relatively simple differential equations, so if I give you
    $\theta(0)$ and $\dot\theta(0)$, then you should be able to integrate them
    to obtain $\theta(t)$... right?  Although it is possible, integrating even
    the simplest case ($b = u = 0$) involves elliptic integrals of the first
    kind; there is relatively little intuition to be gained here. </p>

    <p> This is in stark contrast to the case of linear systems, where much of
    our understanding comes from being able to explicitly integrate the
    equations. For instance, for a simple linear system we have $$\dot{q} = a q
    \quad \rightarrow \quad q(t) = q(0) e^{at},$$ and we can immediately
    understand that the long-term behavior of the system is a (stable) decaying
    exponential if $a<0$, an (unstable) growing exponential if $a>0$, and that
    the system does nothing if $a=0$. Here we are with certainly one of the
    simplest nonlinear systems we can imagine, and we can't even solve this
    system? </p>

    <p> All is not lost.  If what we care about is the long-term behavior of the
    system, then there are a number of techniques we can apply.  In this
    chapter, we will start by investigating graphical solution methods. These
    methods are described beautifully in a book by Steve
    Strogatz<elib>Strogatz94</elib>.</p>

    <section data-type="sect2"><h1>The overdamped pendulum</h1>

      <p> Let's start by studying a special case -- intuitively when
      $b\dot\theta \gg ml^2\ddot\theta$ -- which via <a
      href="https://en.wikipedia.org/wiki/Dimensionless_quantity">dimensional
      analysis</a> (using the natural frequency $\sqrt{\frac{g}{l}}$ to match
      units) occurs when $b \sqrt\frac{l}{g} \gg ml^2$. This is the case of
      heavy damping, for instance if the pendulum was moving in molasses.  In
      this case, the damping term dominates the acceleration term, and we have:
      $$ml^2 \ddot\theta + b\dot\theta \approx b\dot\theta = u_0 -
      mgl\sin\theta.$$ In other words, in the case of heavy damping, the system
      looks approximately first order. This is a general property of heavily
      damped systems, such as fluids at very low Reynolds number. </p>

      <p> I'd like to ignore one detail for a moment: the fact that $\theta$
      wraps around on itself every $2\pi$.  To be clear, let's write the system
      without the wrap-around as: \begin{equation}b\dot{x} = u_0 -
      mgl\sin{x}.\label{eq:overdamped_pend_ct}\end{equation} Our goal is to
      understand the long-term behavior of this system: to find $x(\infty)$
      given $x(0)$.  Let's start by plotting $\dot{x}$ vs $x$ for the case when
      $u_0=0$:</p>

      <figure> <img width="70%" data-src="figures/pend_sinx.svg"/> </figure>

      <p> The first thing to notice is that the system has a number of <em>fixed
      points</em> or <em>steady states</em>, which occur whenever $\dot{x} = 0$.
      In this simple example, the zero-crossings are $x^* = \{..., -\pi, 0, \pi,
      2\pi, ...\}$. When the system is in one of these states, it will never
      leave that state.  If the initial conditions are at a fixed point, we know
      that $x(\infty)$ will be at the same fixed point.</p>

      <p>Next let's investigate the behavior of the system in the local vicinity
      of the fixed points.  Examining the fixed point at $x^* = \pi$, if the
      system starts just to the right of the fixed point, then $\dot{x}$ is
      positive, so the system will move away from the fixed point.  If it starts
      to the left, then $\dot{x}$ is negative, and the system will move away in
      the opposite direction.  We'll call fixed-points which have this property
      <em>unstable</em>.  If we look at the fixed point at $x^* = 0$, then the
      story is different: trajectories starting to the right or to the left will
      move back towards the fixed point.  We will call this fixed point
      <em>locally stable</em>.  More specifically, we'll distinguish between
      three types of local stability:

      <ul>

        <li>Locally stable <em>in the sense of Lyapunov</em> (i.s.L.).  A fixed
        point, $x^*$ is locally stable i.s.L. if for every small $\epsilon > 0$,
        I can produce a $\delta > 0$ such that if $\| x(0) - x^* \| < \delta$
        then $\forall t$ $\| x(t) - x^*\| < \epsilon$.  In words, this means
        that for any ball of size $\epsilon$ around the fixed point, I can
        create a ball of size $\delta$ which guarantees that if the system is
        started inside the $\delta$ ball then it will remain inside the
        $\epsilon$ ball for all of time.</li>

        <li>Locally <em>asymptotically stable</em>.  A fixed point is locally
        asymptotically stable if $x(0) = x^* + \epsilon$ implies that
        $\lim_{t\rightarrow \infty} x(t) = x^*$.</li>

        <li>Locally <em>exponentially stable</em>.  A fixed point is locally
        exponentially stable if $x(0) = x^* + \epsilon$ implies that $\| x(t) -
        x^* \| < Ce^{-\alpha t}$, for some positive constants $C$ and
        $\alpha$.</li>

      </ul>

      <p>An initial condition near a fixed point that is stable in the sense of
      Lyapunov may never reach the fixed point (but it won't diverge), near an
      asymptotically stable fixed point will reach the fixed point as $t
      \rightarrow \infty$, and near an exponentially stable fixed point will
      reach the fixed point with a bounded rate.  An exponentially stable fixed
      point is also an asymptotically stable fixed point, but the converse is
      not true.  Asymptotic stability and Lyapunov stability, however, are
      distinct notions -- it is actually possible to have a system that is
      asymptotically stable but not stable i.s.L.&dagger;<sidenote>&dagger; we
      can't see that in one dimension so will have to hold that example for a
      moment</sidenote>.  <!-- imagine a pendulum that converges to the top from
      clockwise, but for counter-clockwise it travels all of the way around
      before getting back to the fixed point.  (asymptotically stable, but not
      isL).  To avoid wrap-around coordinate issues, use the x position of the
      endpoint.  see exercise 1.14 in
      http://books.google.com/books?id=ByCCAwAAQBAJ&pg=PA6&lpg=PA6&dq=locally+attractive+but+not++lyapunov+stable&source=bl&ots=BLlwzfFryl&sig=lDXX4oFzIhXEH7do-4llBXU3NKo&hl=en&sa=X&ei=tqEVVKDEGoqYyATh84G4Cw&ved=0CDkQ6AEwAw#v=onepage&q=locally%20attractive%20but%20not%20%20lyapunov%20stable&f=false
      for a counter-example .  -->  Systems which are stable i.s.L. but not
      asymptotically stable are easy to construct (e.g. $\dot{x} = 0$).
      Interestingly, it is also possible to have nonlinear systems that converge
      (or diverge) in finite-time; a so-called <em>finite-time stability</em>;
      we will see examples of this later in the book, but it is a difficult
      topic to penetrate with graphical analysis.  Rigorous nonlinear system
      analysis is rich with subtleties and surprises.  Moreover, these
      differences actually matter -- the code that we will write to stabilize
      the systems will be subtley different depending on what type of stability
      we want, and it can make or break the success of our methods.</p>

      <p> Our graph of $\dot{x}$ vs. $x$ can be used to convince ourselves of
      i.s.L. and asymptotic stability by visually inspecting $\dot{x}$ in the
      vicinity of a fixed point.  Even exponential stability can be inferred if
      the function can be bounded away from the origin by a negatively-sloped
      line through the fixed point, since it implies that the nonlinear system
      will converge at least as fast as the linear system represented by the
      straight line.  I will graphically illustrate unstable fixed points with
      open circles and stable fixed points (i.s.L.) with filled circles. </p>

      <p>Next, we need to consider what happens to initial conditions which
      begin farther from the fixed points.  If we think of the dynamics of the
      system as a flow on the $x$-axis, then we know that anytime $\dot{x} > 0$,
      the flow is moving to the right, and $\dot{x} < 0$, the flow is moving to
      the left.  If we further annotate our graph with arrows indicating the
      direction of the flow, then the entire (long-term) system behavior becomes
      clear:</p>

      <figure> <img width="70%" data-src="figures/pend_sinx_annotated.svg"/>
      </figure>

      <p> For instance, we can see that any initial condition $x(0) \in
      (-\pi,\pi)$ will result in $\lim_{t\rightarrow \infty} x(t) = 0$.  This
      region is called the <em>basin of attraction</em> of the fixed point at
      $x^* = 0$. Basins of attraction of two fixed points cannot overlap, and
      the manifold separating two basins of attraction is called the
      <em>separatrix</em>.  Here the unstable fixed points, at $x^* = \{..,
      -\pi, \pi, 3\pi, ...\}$ form the separatrix between the basins of
      attraction of the stable fixed points. </p>

      <p> As these plots demonstrate, the behavior of a first-order one
      dimensional system on a line is relatively constrained.  The system will
      either monotonically approach a fixed-point or monotonically move toward
      $\pm \infty$. There are no other possibilities.  Oscillations, for
      example, are impossible. Graphical analysis is a fantastic analysis tool
      for many first-order nonlinear systems (not just pendula); as illustrated
      by the following example: </p>

      <div data-type="example"><h1>Nonlinear autapse</h1> Consider the following
      system: \begin{equation} \dot{x} + x = \tanh(w x), \end{equation} which is
      plotted below for a few values of $w$.  It's convenient to note that
      $\tanh(z) \approx z$ for small $z$.  For $w \le 1$ the system has only a
      single fixed point.  For $w > 1$ the system has three fixed points : two
      stable and one unstable.

      <figure> <img width="80%" data-src="figures/pend_autapse.svg"/> </figure>

      These equations are not arbitrary - they are actually a model for one of
      the simplest neural networks, and one of the simplest model of persistent
      memory<elib>Seung00</elib>.  In the equation $x$ models the firing rate of
      a single neuron, which has a feedback connection to itself.  $\tanh$ is
      the activation (sigmoidal) function of the neuron, and $w$ is the weight
      of the synaptic feedback. </div>

      <p> One last piece of terminology.  In the neuron example, and in many
      dynamical systems, the dynamics were parameterized; in this case by a
      single parameter, $w$.  As we varied $w$, the fixed points of the system
      moved around.  In fact, if we increase $w$ through $w=1$, something
      dramatic happens - the system goes from having one fixed point to having
      three fixed points.  This is called a <em>bifurcation</em>.  This
      particular bifurcation is called a pitchfork bifurcation.  We often draw
      bifurcation diagrams which plot the fixed points of the system as a
      function of the parameters, with solid lines indicating stable fixed
      points and dashed lines indicating unstable fixed points, as seen in the
      figure:</p>

      <figure> <todo>bifurcation diagram asymptotes to $x^* = 1$</todo> <img
      width="80%" data-src="figures/pend_autapse_bifurcation.svg"/>
      <figcaption>Bifurcation diagram of the nonlinear autapse.</figcaption>
      </figure>

      <p> Our pendulum equations also have a (saddle-node) bifurcation when we
      change the constant torque input, $u_0$.  <!-- This is the subject of
      exercise~\ref{p:pend_bifurcation}.-->  Finally, let's return to the
      original equations in $\theta$, instead of in $x$.  Only one point to
      make: because of the wrap-around, this system will <em>appear</em> have
      oscillations.  In fact, the graphical analysis reveals that the pendulum
      will turn forever whenever $|u_0| > \frac{mgl}{b}$, but now you understand
      that this is not an oscillation, but an instability with $\theta
      \rightarrow \pm \infty$. </p>

    </section> <!-- end of overdamped pend -->

    <section data-type="sect2"><h1>The undamped pendulum with zero torque</h1>

      <p> Consider again the system $$ml^2 \ddot\theta = u_0 - mgl \sin\theta -
      b\dot\theta,$$ this time with $b = 0$.  This time the system dynamics are
      truly second-order.  We can always think of any second-order system as
      (coupled) first-order system with twice as many variables. Consider a
      general, autonomous (not dependent on time), second-order system,
      $$\ddot{q} = f(q,\dot q,u).$$ This system is equivalent to the
      two-dimensional first-order system \begin{align*} \dot x_1 =& x_2 \\ \dot
      x_2 =& f(x_1,x_2,u), \end{align*} where $x_1 = q$ and $x_2 = \dot q$.
      Therefore, the graphical depiction of this system is not a line, but a
      vector field where the vectors $[\dot x_1, \dot x_2]^T$ are plotted over
      the domain $(x_1,x_2)$.  This vector field is known as the <em>phase
      portrait</em> of the system.</p>

      <p> In this section we restrict ourselves to the simplest case when $u_0 =
      0$. Let's sketch the phase portrait.  First sketch along the
      $\theta$-axis. The $x$-component of the vector field here is zero, the
      $y$-component is $-mgl\sin\theta.$ As expected, we have fixed points at
      $\pm \pi, ...$ Now sketch the rest of the vector field.  Can you tell me
      which fixed points are stable? Some of them are stable i.s.L., none are
      asymptotically stable.</p>

      <figure> <img width="80%" data-src="figures/pend_undamped_phase.svg"/>
      </figure>

      <section data-type="sect3"><h1>Orbit calculations</h1>

        <p> You might wonder how we drew the black contour lines in the figure
        above.  We could have obtained them by simulating the system
        numerically, but those lines can be easily obtained in closed-form.
        Directly integrating the equations of motion is difficult, but at least
        for the case when $u_0 = 0$, we have some additional physical insight
        for this problem that we can take advantage of.  The kinetic energy,
        $T$, and potential energy, $U$, of the pendulum are given by $$T =
        \frac{1}{2}I\dot\theta^2, \quad U = -mgl\cos(\theta),$$ where $I=ml^2$
        and the total energy is $E(\theta,\dot\theta) =
        T(\dot\theta)+U(\theta)$.  The undamped pendulum is a conservative
        system: total energy is a constant over system trajectories.  Using
        conservation of energy, we have:

        \begin{gather*}  E(\theta(t),\dot\theta(t)) = E(\theta(0),\dot\theta(0))
        = E_0 \\ \frac{1}{2} I \dot\theta^2(t) - mgl\cos(\theta(t)) = E_0  \\
        \dot\theta(t) = \pm \sqrt{\frac{2}{I}\left[E_0 +
        mgl\cos\left(\theta(t)\right)\right]} \end{gather*}

        Using this, if you tell me $\theta$ I can determine one of two possible
        values for $\dot\theta$, and the solution has all of the richness of the
        black countour lines from the plot.  This equation has a real solution
        when $\cos(\theta) > \cos(\theta_{max})$, where $$\theta_{max} =
        \begin{cases} \cos^{-1}\left( \frac{E}{mgl} \right), & E < mgl \\ \pi, &
        \text{otherwise}. \end{cases}$$ Of course this is just the intuitive
        notion that the pendulum will not swing above the height where the total
        energy equals the potential energy.  As an exercise, you can verify that
        differentiating this equation with respect to time indeed results in the
        equations of motion.</p>

      </section>

<!--  these are correct, but take away from the flow.  move to an appendix?

<section data-type="sect3"><h1>Trajectory calculations</h1>

<p> For completeness, I'll include what it would take to solve for $\theta(t)$,
even thought it cannot be accomplished using elementary functions.  Feel free to
skip this subsection.  We begin the integration with

\begin{gather*} \frac{d\theta}{dt} = \sqrt{\frac{2}{I}\left[E +
mgl\cos\left(\theta(t)\right)\right]} \\ \int_{\theta(0)}^{\theta(t)}
\frac{d\theta}{\sqrt{\frac{2}{I}\left[E +
mgl\cos\left(\theta(t)\right)\right]}} = \int_0^t dt' = t \end{gather*}

The
integral on the left side of this equation is an (incomplete) elliptic integral
of the first kind.  Using the identity: $$\cos(\theta) = 1 - 2
\sin^2(\frac{1}{2}\theta),$$ and manipulating, we have $$t =
\sqrt{\frac{I}{2(E+mgl)}} \int_{\theta(0)}^{\theta(t)} \frac{d\theta}{\sqrt{1 -
k_1^2\sin^2(\frac{\theta}{2})}}, \quad \text{with
}k_1=\sqrt{\frac{2mgl}{E+mgl}}.$$ In terms of the incomplete elliptic integral
function, $$F(\phi,k) = \int_0^\phi \frac{d\theta}{\sqrt{1-k^2\sin^2\theta}},$$
accomplished by a change of variables.  If $E <= mgl$, which is the case of
closed-orbits, we use the following change of variables to ensure $ 0 < k < 1 $ :
\begin{gather*}\phi = \sin^{-1}\left[ k_1 \sin\left( \frac{\theta}{2} \right)
\right] \\ \cos(\phi) d\phi = \frac{1}{2} k_1 \cos\left(\frac{\theta}{2}\right)
d\theta = \frac{1}{2} k_1 \sqrt{1 - \frac{\sin^2 (\phi)}{k_1^2}} d\theta
\end{gather*} so we have
\begin{gather*}
t = \frac{1}{k_1}\sqrt{\frac{2I}{(E+mgl)}} \int_{\phi(0)}^{\phi(t)}
\frac{d\phi}{\sqrt{1 - \sin^2(\phi)}} \frac{\cos(\phi)}{\sqrt{1 -
\frac{\sin^2\phi}{k_1^2}}} \\ = \sqrt{\frac{I}{mgl}} \left[
F\left(\phi(t),k_2\right) - F\left(\phi(0),k_2\right) \right],\quad
k_2 = \frac{1}{k_1}.\end{gather*} The inverse of $F$ is given by the
Jacobi elliptic functions (sn,cn,...), yielding:
<!-- http://en.wikipedia.org/wiki/Jacobi%27s_elliptic_functions -->
<!--
\begin{gather*}\sin(\phi(t)) = \text{sn}\left(t
  \sqrt{\frac{mgl}{I}} + F\left(\phi(0),k_2\right),k_2 \right) \\
\theta(t) = 2\sin^{-1} \left[ k_2 \text{sn}\left(t
  \sqrt{\frac{mgl}{I}} + F\left(\phi(0),k_2\right),k_2 \right) \right]
\end{gather*}
The function $\text{sn}$ used here can be evaluated in MATLAB by
calling $$\text{sn}(u,k) = \text{ellipj}(u,k^2).$$ The function
$F$ is not implemented in MATLAB, but implementations can be
downloaded. (note that $F(0,k) = 0$).</p>

<p>
For the open-orbit case, $E>mgl$, we use $$\phi =
\frac{\theta}{2},\quad \frac{d\phi}{d\theta} = \frac{1}{2},$$ yielding
\begin{gather*}
t = \frac{2I}{E+mgl} \int_{\phi(0)}^{\phi(t)} \frac{d\phi}{\sqrt{1 -
    k_1^2 \sin^2(\phi)}} \\
\theta(t) = 2 \tan^{-1} \left[ \frac{ \text{sn} \left( t
    \sqrt{\frac{E+mgl}{2I}} + F\left( \frac{\theta(0)}{2}, k_1 \right)
    \right) } { \text{cn} \left( t
    \sqrt{\frac{E+mgl}{2I}} + F\left( \frac{\theta(0)}{2}, k_1 \right)
    \right) }
 \right]
\end{gather*}
Notes: Use MATLAB's <code>atan2</code> and <code>unwrap</code> to recover the
complete trajectory.</p>

<!-- primary refs:
  http://en.wikipedia.org/wiki/Pendulum_%28mathematics%29
  http://kr.cs.ait.ac.th/~radok/math/mat6/calc81.htm
  http://books.google.com/books?id=xWrJlTIYl_IC&pg=PA280&lpg=PA280&dq=pendulum+elliptic+integrals&source=web&ots=zas9k5qVc0&sig=Tz-OwdqS84VPM2I_pZbtaspVZNk#PPA280,M1 (Computational Physics: Problem Solving with Computers by Rubin H. Landau, Cristian C. Bordeianu, p.280)
  http://en.wikipedia.org/wiki/Binomial_theorem
  http://mathworld.wolfram.com/EllipticIntegraloftheFirstKind.html -->


<!--</section> -->

    </section> <!-- end of undamped pend -->

    <section data-type="sect2"><h1>The undamped pendulum with a constant
    torque</h1>

      <p> Now what happens if we add a constant torque?  If you visualize the
      bifurcation diagram, you'll see that the fixed points come together,
      towards $q = \frac{\pi}{2}, \frac{5\pi}{2}, ...$, until they disappear.
      One fixed-point is unstable, and one is stable.</p>

      <!-- todo: find a way to add an animation or something in here.  do a
      python demo in class -->

    </section> <!-- undamped constant torque -->

    <p>Before we continue, let me now give you the promised example of a system
    that is asymptotically stable, but not stable i.s.L.. We can accomplish this
    with a very pendulum-like example (written here in polar coordinates):</p>

    <div data-type="example"><h1>Asymptotically stable, but not Lyapunov
    stable</h1>

      <p>The system \[ \dot{r} = r(1-r) \\ \dot\theta = \sin^2
      (\frac{\theta}{2}) \] is asymptotically stable to $x^* = [1,0]^T$, but is
      not stable i.s.L.</p>

    </div>

    <p>Take a minute to draw the vector field of this (you can draw each
    coordinate independently, if it helps) to make sure you understand.  Note
    that to wrap-around rotation is convenient but not essential -- we could
    have written the same dynamical system in cartesian coordinates without
    wrapping.  And if this feel too arbitrary, we will see it happen in practice
    when we introduce the energy-shaping swing-up controller for the pendulum in
    the next chapter.</p>

    <section data-type="sect2"><h1>The damped pendulum</h1>

      <p>Now let's add damping back.  You can still add torque to move the
      fixed points (in the same way).</p>

      <figure>
        <img width="90%" data-src="figures/pend_damped_phase.svg"/>
        <figcaption>Phase diagram for the damped pendulum</figcaption>
      </figure>

      <p>With damping, the downright fixed point of the pendulum now becomes
      asymptotically stable (in addition to stable i.s.L.). Is it also
      exponentially stable?  How can we tell?  One technique is to linearize the
      system at the fixed point.  A smooth, time-invariant, nonlinear system
      that is locally exponentially stable <i>must</i> have a stable
      linearization; we'll discuss linearization more in the next chapter. </p>

      <p>Here's a thought exercise.  If $u$ is no longer a constant, but a
      function $\pi(\theta,\dot{\theta})$, then how would you choose $\pi$ to
      stabilize the vertical position.  Feedback linearization is the trivial
      solution, for example: $$u = \pi(\theta,\dot{\theta}) = 2mgl\sin\theta.$$
      But these plots we've been making tell a different story.  How would you
      shape the natural dynamics - at each point pick a $u$ from the stack of
      phase plots - to stabilize the vertical fixed point <em>with minimal
      torque effort</em>?  This is exactly the way that I would like you to
      think about control system design.  And we'll give you your first solution
      techniques -- using dynamic programming -- in the next lecture.</p>

    </section> <!-- damped pend -->

  </section> <!-- constant torque -->

  <section data-type="sect1"><h1>The torque-limited simple pendulum</h1>

    <p>The simple pendulum is fully actuated.  Given enough torque, we can
    produce any number of control solutions to stabilize the originally unstable
    fixed point at the top (such as designing a feedback law to effectively
    invert gravity).</p>

    <p>The problem begins to get interesting (a.k.a. becomes underactuated)
    if we impose a torque-limit constraint, $|u|\le u_{max}$.  Looking at the
    phase portraits again, you can now visualize the control problem.  Via
    feedback, you are allowed to change the direction of the vector field at
    each point, but only by a fixed amount.  Clearly, if the maximum torque is
    small (smaller than $mgl$), then there are some states which cannot be
    driven directly to the goal, but must pump up energy to reach the goal.
    Furthermore, if the torque-limit is too severe and the system has damping,
    then it may be impossible to swing up to the top.  The existence of a
    solution, and number of pumps required to reach the top, is a non-trivial
    function of the initial conditions and the torque-limits.</p>

    <p>Although this system is very simple, its solution requires much of the
    same reasoning necessary for controlling much more complex underactuated
    systems; this problem will be a work-horse for us as we introduce new
    algorithms throughout this book.</p>

  </section>

</section> <!-- ***************  end of pendulum chapter **************   -->

<section data-type="chapter" class="chapter" id="acrobot">
  <h1>Acrobots, Cart-Poles, and Quadrotors</h1>

  <p> A great deal of work in the control of underactuated systems has been
  done in the context of low-dimensional model systems.  These model   systems
  capture the essence of the problem without introducing all of   the complexity
  that is often involved in more real-world examples. In   this chapter we will
  focus on two of the most well-known and   well-studied model systems--the
  Acrobot and the Cart-Pole.  After we have developed some tools, we will see
  that they can be applied directly to other model systems; we will give a
  number of examples using Quadrotors.  All of these systems are trivially
  underactuated, having less actuators than degrees of freedom.</p>

  <section data-type="sect1"><h1>The Acrobot</h1>

    <p> The Acrobot is a planar two-link robotic arm in the vertical plane
    (working against gravity), with an actuator at the elbow, but no actuator at
    the shoulder.  It was first described in detail in <elib>Murray91</elib>.
    The companion system, with an actuator at the shoulder but not at the elbow,
    is known as the Pendubot<elib>Spong97</elib>.  The Acrobot is so named
    because of its resemblance to a gymnast (or acrobat) on a parallel bar, who
    controls his/her motion predominantly by effort at the waist (and not effort
    at the wrist).  The most common control task studied for the Acrobot is the
    swing-up task, in which the system must use the elbow (or waist) torque to
    move the system into a vertical configuration then balance. </p>

    <figure>
    <img width="40%" data-src="figures/acrobot.svg"/>
    <figcaption>The Acrobot.  <a href="http://groups.csail.mit.edu/locomotion/drake/movies/acrobot_swingup.swf">Click here for an animation of the swing-up task</a> or <a href="http://youtu.be/FeCwtvrD76I">here to see a real robot</a>.</figcaption>
    </figure>

    <p> The Acrobot is representative of the primary challenge in underactuated
    robots.  In order to swing up and balance the entire system, the controller
    must reason about and exploit the state-dependent coupling between the
    actuated degree of freedom and the unactuated degree of freedom.  It is also
    an important system because, as we will see, it closely resembles one of the
    simplest models of a walking robot.</p>

    <section data-type="sect2"><h1>Equations of motion</h1>

      <p> Figure 3.1 illustrates the model parameters used in our analysis.
      $\theta_1$ is the shoulder joint angle, $\theta_2$ is the elbow (relative)
      joint angle, and we will use $\bq = [\theta_1,\theta_2]^T$, $\bx =
      [\bq,\dot\bq]^T$.  The zero configuration is with both links pointed
      directly down.  The moments of inertia, $I_1,I_2$ are taken about the
      pivots<!--<elib>Spong95</elib> uses the center of mass, which differs only
      by an extra term in each inertia from the parallel axis theorem.-->.  The
      task is to stabilize the unstable fixed point $\bx = [\pi,0,0,0]^T$.</p>

      <p>
      We will derive the equations of motion for the Acrobot using the
      method of Lagrange.  The kinematics are given by:
      \begin{equation}
      \bx_1 = \begin{bmatrix} l_1 s_1 \\ -l_1 c_1
        \end{bmatrix}, \quad \bx_2 = \bx_1 + \begin{bmatrix} l_2 s_{1+2} \\
        - l_2 c_{1+2} \end{bmatrix} .
      \end{equation}
      The energy <!--\footnote{The complicated expression for $T_2$ comes from
        $T_2 = \frac{1}{2} m_2 v_{c2}^2 + \frac{1}{2}I_{c2} (\dot{q}_1 +
        \dot{q}_2)^2$, where $v_{c2}$ is the velocity of the center of mass
        of the second link and $I_{c2} = I_2 - m_2 l_{c2}^2$ is the moment
        of inertia about that point.  Use $c_1 c_{1+2} + s_1 s_{1+2} =
        c_2$ to simplify.}--> is given by:
      \begin{gather}
      T = T_1 + T_2, \quad T_1 = \frac{1}{2} I_1 \dot{q}_1^2 \\
      T_2 = \frac{1}{2} ( m_2 l_1^2 + I_2 + 2 m_2 l_1 l_{c2} c_2 )
      \dot{q}_1^2 + \frac{1}{2} I_2 \dot{q}_2^2 + (I_2 + m_2 l_1 l_{c2} c_2
      ) \dot{q}_1 \dot{q}_2 \\
      % from expanding sum of point masses
      U = -m_1 g l_{c1} c_1 - m_2 g (l_1 c_1 + l_{c2} c_{1+2})
      \end{gather}
      Entering these quantities into the Lagrangian yields the equations of
      motion:
      \begin{gather}
      (I_1 + I_2 + m_2 l_1^2 + 2m_2 l_1 l_{c2} c_2) \ddot{q}_1 + (I_2 + m_2
        l_1 l_{c2} c_2)\ddot{q}_2 - 2m_2 l_1 l_{c2} s_2 \dot{q}_1 \dot{q}_2
        \\ \quad -m_2 l_1 l_{c2} s_2 \dot{q}_2^2 + m_1 g l_{c1}s_1 + m_2 g
        (l_1 s_1 + l_{c2} s_{1+2}) = 0 \\
      (I_2 + m_2 l_1 l_{c2} c_2) \ddot{q}_1 + I_2 \ddot{q}_2 + m_2 l_1
        l_{c2} s_2 \dot{q}_1^2 + m_2 g l_{c2} s_{1+2} = \tau
      \end{gather}
      In standard, manipulator equation form:
      $$\bM(\bq)\ddot\bq + \bC(\bq,\dot\bq)\dot\bq = \btau_g(\bq) +
      \bB\bu,$$ using $\bq = [\theta_1,\theta_2]^T$, $\bu = \tau$ we have:
      \begin{gather}
      \bM(\bq) = \begin{bmatrix} I_1 + I_2 + m_2 l_1^2 + 2m_2 l_1 l_{c2}
        c_2 & I_2 + m_2 l_1 l_{c2} c_2 \\ I_2 + m_2 l_1 l_{c2} c_2 & I_2
        \end{bmatrix},\label{eq:Hacrobot}\\
      \bC(\bq,\dot{\bq}) = \begin{bmatrix} -2 m_2
        l_1 l_{c2} s_2 \dot{q}_2 & -m_2 l_1 l_{c2} s_2 \dot{q}_2 \\ m_2 l_1
        l_{c2} s_2 \dot{q}_1 & 0 \end{bmatrix}, \\
      \btau_g(\bq) = \begin{bmatrix} -m_1 g l_{c1}s_1 -
        m_2 g (l_1 s_1 + l_{c2}s_{1+2}) \\
        -m_2 g l_{c2} s_{1+2} \end{bmatrix}, \quad
      \bB = \begin{bmatrix} 0 \\ 1 \end{bmatrix}.
      \end{gather}
      </p>
      <!--
      Solving directly for the accelerations, we have
      \begin{gather}
      \det(\bM) = I_1 I_2 + m_2 l_1^2 (1 - m_2 l_{c2}^2 c_2^2)
      \\
      \ddot{q}_1 = \frac{1}{det(\bM)} \left[ I_2 \left( m_2 l_1 l_{c2} s_2
        (2 \dot{q}_1 + \dot{q}_2) \dot{q}_2 - (m_1 l_{c1} + m_2 l_1) g s_1 -
        m_2 g l_{c2} s_{1+2} \right) + (I_{c2} + m_2 l_1 l_{c2} c_2) \left( -m_2
        l_1 l_{c2} s_2 \dot{q}_1^2 - m_2 g l_{c2} s_{1+2} \right)
        \right]
      \end{gather} -->

      <div data-type="example" class="drake"><h1>The Acrobot in Python</h1>

      <p>You can experiment with the Acrobot dynamics in <drake></drake> using,
        e.g.</p>

      <pysrc>acrobot/torque_slider_demo.py</pysrc>

      </div>

    </section> <!-- acrobot eqs of motion -->

  </section> <!-- acrobot -->

  <section data-type="sect1"><h1>The Cart-Pole system</h1>

    <p> The other model system that we will investigate here is the cart-pole
    system, in which the task is to balance a simple pendulum around its
    unstable equilibrium, using only horizontal forces on the   cart.  Balancing
    the cart-pole system is used in many introductory   courses in control,
    including 6.003 at MIT, because it can be   accomplished with simple linear
    control (e.g. pole placement)   techniques.  In this chapter we will
    consider the full swing-up and   balance control problem, which requires a
    full nonlinear control treatment.</p>

    <figure>
      <img width="50%" data-src="figures/cartpole.svg"/>
      <figcaption>The Cart-Pole system.  Click <a href="http://youtu.be/Bzq96V1yN5k">here to see a real robot</a>.</figcaption>
      <todo> add swing-up + balance swf</todo>
    </figure>

    <p> The figure shows our parameterization of the system. $x$   is the
    horizontal position of the cart, $\theta$ is the   counter-clockwise angle
    of the pendulum (zero is hanging straight   down).  We will use $\bq =
    [x,\theta]^T$, and $\bx = [\bq,\dot\bq]^T$.   The task is to stabilize the
    unstable fixed point at $\bx = [0,\pi,0,0]^T.$ </p>

    <section data-type="sect2"><h1>Equations of motion</h1>

      <p>
      The kinematics of the system are given by
      \begin{equation}\bx_1 = \begin{bmatrix} x \\ 0
      \end{bmatrix}, \quad \bx_2 = \begin{bmatrix} x + l\sin\theta \\
      -l\cos\theta \end{bmatrix}. \end{equation}
      The energy is given by
      \begin{align} T=& \frac{1}{2} (m_c + m_p)\dot{x}^2 + m_p
      \dot{x}\dot\theta l \cos{\theta} + \frac{1}{2}m_p l^2 \dot\theta^2 \\
      U =& -m_p g l \cos\theta. \end{align}
      The Lagrangian yields the equations of motion:
      \begin{gather}
      (m_c + m_p)\ddot{x} + m_p l \ddot\theta \cos\theta - m_p l \dot\theta^2 \sin\theta = f \\
      m_p l \ddot{x} \cos\theta + m_p l^2 \ddot\theta + m_p g l \sin\theta = 0
      \end{gather}
      In standard, manipulator equation form:
      $$\bM(\bq)\ddot\bq + \bC(\bq,\dot\bq)\dot\bq = \btau_g(\bq) +
      \bB\bu,$$ using $\bq = [x,\theta]^T$, $\bu = f$, we have:
      \begin{gather*}
      \bM(\bq) = \begin{bmatrix} m_c + m_p & m_p l \cos\theta \\ m_p l
        \cos\theta & m_p l^2 \end{bmatrix}, \quad \bC(\bq,\dot{\bq}) =
      \begin{bmatrix} 0 & -m_p l \dot\theta \sin\theta \\ 0 & 0
      \end{bmatrix}, \\
      \btau_g(\bq) = \begin{bmatrix} 0 \\ - m_p gl \sin\theta \end{bmatrix},
      \quad \bB = \begin{bmatrix} 1 \\ 0 \end{bmatrix}
      \end{gather*}
      In this case, it is particularly easy to solve directly for the accelerations:
      \begin{align}
      \ddot{x} =& \frac{1}{m_c + m_p \sin^2\theta}\left[ f+m_p \sin\theta (l \dot\theta^2 + g\cos\theta)\right] \label{eq:ddot_x}\\
      \ddot{\theta} =& \frac{1}{l(m_c + m_p \sin^2\theta)} \left[ -f
       \cos\theta - m_p l \dot\theta^2 \cos\theta \sin\theta - (m_c + m_p) g \sin\theta \right] \label{eq:ddot_theta}
      \end{align}
      In some of the analysis that follows, we will study the form of
      the equations of motion, ignoring the details, by arbitrarily setting
      all constants to 1:
      \begin{gather}
      2\ddot{x} + \ddot\theta \cos\theta - \dot\theta^2 \sin\theta = f \label{eq:simple}\\
      \ddot{x}\cos\theta + \ddot\theta + \sin\theta = 0. \label{eq:simple2}
      \end{gather}
      </p>

      <div data-type="example" class="drake"><h1>The Cart-Pole System in
      Python</h1>

      <p>You can experiment with the Cart-Pole dynamics in <drake></drake> using,
        e.g.</p>

      <pysrc>cartpole/force_slider_demo.py</pysrc>

      </div>

    </section> <!-- end eq of motion -->

  </section> <!-- end cart-pole -->

  <section data-type="sect1"><h1>Balancing</h1>

    <p> For both the Acrobot and the Cart-Pole systems, we will begin by
    designing a linear controller which can balance the system when it begins in
    the vicinity of the unstable fixed point.  To accomplish this, we will
    linearize the nonlinear equations about the fixed point, examine the
    controllability of this linear system, then using <a
    href="?chapter=lqr">linear quadratic regulator (LQR)</a> theory to design
    our feedback controller.</p>

    <section data-type="sect2"><h1>Linearizing the manipulator equations</h1>

      <p>Although the equations of motion of both of these model systems are
      relatively tractable, the forward dynamics still involve quite a few
      nonlinear terms that must be considered in any linearization.  Let's
      consider the general problem of linearizing a system described by the
      manipulator equations.</p>

      <p>We can perform linearization around a fixed point, $(\bx^*, \bu^*)$,
      using a Taylor expansion:

      \begin{equation}
      \dot\bx = {\bf f}(\bx,\bu) \approx {\bf f}(\bx^*,\bu^*) + \left[ \pd{\bf
        f}{\bx}\right]_{\bx=\bx^*,\bu=\bu^*} (\bx - \bx^*) + \left[ \pd{\bf
        f}{\bu}\right]_{\bx=\bx^*,\bu=\bu^*} (\bu - \bu^*)
      \end{equation}

      Let us consider the specific problem of linearizing the manipulator
      equations around a (stable or unstable) fixed point.  In this case, ${\bf
      f}(\bx^*,\bu^*)$ is zero, and we are left with the standard linear
      state-space form:

      \begin{align}
      \dot\bx =& \begin{bmatrix} \dot\bq \\ \bM^{-1}(\bq) \left[ \btau_g(\bq) + {\bf
            B}(\bq)\bu - \bC(\bq,\dot\bq)\dot\bq \right]
            \end{bmatrix},\\
      \approx& {\bf A}_{lin} (\bx-\bx^*) + \bB_{lin} (\bu - \bu^*),
      \end{align}

      where ${\bf A}_{lin}$, and $\bB_{lin}$ are constant matrices.  Let us
      define $\bar\bx = \bx - \bx^*, \bar\bu = \bu - \bu^*$, and write
      $$\dot{\bar\bx} = {\bf A}_{lin}\bar\bx + \bB_{lin}\bar\bu.$$ Evaluation of
      the Taylor expansion around a fixed point yields the following, very
      simple equations, given in block form by:

      \begin{align}
      {\bf A}_{lin} =& \begin{bmatrix} {\bf 0} & {\bf I} \\ \bM^{-1} \pd{\btau_g}{\bq} + \sum_{j} \bM^{-1}\pd{\bB_j}{\bq} u_j  & {\bf 0} \end{bmatrix}_{\bx=\bx^*,\bu=\bu^*} \\
      \bB_{lin} =& \begin{bmatrix} {\bf 0} \\ \bM^{-1} \bB
        \end{bmatrix}_{\bx=\bx^*, \bu=\bu^*}
      \end{align}

      where $\bB_j$ is the $j$th column of $\bB$.  Note that the term involving
      $\pd{\bM^{-1}}{q_i}$ disappears because $\btau_g + \bB\bu - \bC\dot{\bq}$
      must be zero at the fixed point.  All of the $\bC\dot\bq$ derivatives drop
      out, too, because $\dot{\bq}^* = 0$, and any terms with $\bC$ drop out as
      well, since centripetal and centrifugal forces are zero at when velocity
      is zero.  In many cases, including both the Acrobot and Cart-Pole systems
      (but not the Quadrotors), the matrix $\bB(\bq)$ is a constant, so the
      $\pd\bB{\bq}$ terms also drop out. </p>

      <div data-type="example"><h1>Linearization of the Acrobot</h1>

        <p>Linearizing around the (unstable) upright point, we have:

        \begin{gather}
        \left[\pd{\bf \tau_g}{\bq}\right]_{\bx=\bx^*} = \begin{bmatrix} g (m_1 l_{c1}
            + m_2 l_1 + m_2 l_{c2}) & m_2 g l_{c2} \\ m_2 g l_{c2} & m_2 g l_{c2}
        \end{bmatrix}
        \end{gather}

        The linear dynamics follow directly from these equations and the manipulator
        form of the Acrobot equations.</p>

      </div>

      <div data-type="example"><h1>Linearization of the Cart-Pole System</h1>

      <p>Linearizing around the unstable fixed point in this system, we have:

        \begin{gather}
        \left[\pd{\btau_g}{\bq}\right]_{\bx=\bx^*} = \begin{bmatrix} 0 & 0 \\ 0
          & m_p g l \end{bmatrix}
        \end{gather}

        Again, the linear dynamics follow simply.</p>

      </div>

      <p>Studying the properties of the linearized system can tell us some
      things about the (local) properties of the nonlinear system.  For
      instance, having a stable linearization implies local exponential
      stability of the nonlinear system<elib>Slotine90+Hauser94a</elib>.   It's
      worth noting that having an unstable linearization also implies that the
      system is locally unstable, but if the linearization is marginally stable
      then one cannot conclude whether the nonlinear system is asymptotically
      stable, stable i.s.L., or unstable (only that it is not exponentially
      stable)<elib>Slotine90</elib>.</p>

    </section> <!-- end linearizing -->

    <section data-type="sect2"><h1>Controllability of linear systems</h1>

      <div data-type="definition"><h1>Controllability</h1>     A control system
      of the form $$\dot{\bx} = {\bf f}(\bx,\bu)$$ is called
      <em>controllable</em> if it is possible to construct an unconstrained
      input signal, $\bu(t)$, $t \in [0,t_f],$ which will move the system from
      any initial state to any final state in a finite interval of time, $0 < t
      < t_f$ <elib>Ogata96</elib>. </div>

      <p> For the linear system $$\dot{\bx} = {\bf A}\bx + \bB\bu,$$ we can
      integrate this linear system in closed form, so it is possible to derive
      the exact conditions of controllability.  In particular, for linear
      systems it is sufficient to demonstrate that there exists a control input
      which drives any initial condition to the origin. </p>

      <section data-type="sect3"><h1>The special case of non-repeated
      eigenvalues</h1>

        <p> Let us first examine a special case, which falls short as a general
        tool but may be more useful for understanding the intuition of
        controllability. Let's perform an eigenvalue analysis of the system
        matrix ${\bf A}$, so that: $${\bf A}{\bf v}_i = \lambda_i {\bf v}_i,$$
        where $\lambda_i$ is the $i$th eigenvalue, and ${\bf v}_i$ is the
        corresponding (right) eigenvector. There will be $n$ eigenvalues for the
        $n \times n$ matrix ${\bf A}$. Collecting the (column) eigenvectors into
        the matrix ${\bf V}$ and the eigenvalues into a diagonal matrix ${\bf
        \Lambda}$, we have $${\bf A}{\bf V} = {\bf V}{\bf \Lambda}.$$ Here comes
        our primary assumption: let us assume that each of these $n$ eigenvalues
        takes on a distinct value (no repeats). With this assumption, it can be
        shown that the eigenvectors ${\bf v}_i$ form a linearly independent
        basis set, and therefore ${\bf V}^{-1}$ is well-defined.</p>

        <p> We can continue our eigenmodal analysis of the linear system by
        defining the modal coordinates, ${\bf r}$, with: $$\bx = {\bf V}{\bf
        r},\quad \text{or}\quad {\bf r} = {\bf V}^{-1}\bx.$$  In modal
        coordinates, the dynamics of the linear system are given by $$\dot{\bf
        r} = {\bf V}^{-1} {\bf A} {\bf V} {\bf r} + {\bf V}^{-1} \bB \bu = {\bf
        \Lambda} {\bf r} + {\bf V}^{-1}\bB \bu.$$  This illustrates the power of
        modal analysis; in modal coordinates, the dynamics diagonalize yielding
        independent linear equations: $$\dot{r}_i = \lambda_i r_i + \sum_j
        \beta_{ij} u_j,\quad {\bf \beta} = {\bf V}^{-1} \bB.$$</p>

        <p> Now the concept of controllability becomes clear.  Input $j$ can
        influence the dynamics in modal coordinate $i$ if and only if ${\bf
        \beta}_{ij} \neq 0$.  In the special case of non-repeated eigenvalues,
        having control over each individual eigenmode is sufficient to (in
        finite time) regulate all of the eigenmodes<elib>Ogata96</elib>.
        Therefore, we say that the system is controllable if and only if
        $$\forall i, \exists j \text{ such that }\beta_{ij} \neq 0.$$</p>

      </section>

      <section data-type="sect3"><h1>A general solution</h1>

        <details><summary>Included only for completeness.  Click to expand the details.</summary>

        <p> A more general solution to the controllability issue, which removes
        our assumption about the eigenvalues, can be obtained by examining the
        time-domain solution of the linear equations.  The solution of this
        system is $$\bx(t) = e^{{\bf A}t} \bx(0) + \int_0^{t} e^{{\bf A}(t -
        \tau)} \bB \bu(\tau) d\tau.$$ Without loss of generality, lets consider
        the that the final state of the system is zero.  Then we have: $$\bx(0)
        = - \int_0^{t_f} e^{-{\bf A}\tau}\bB \bu(\tau) d\tau.$$ You might be
        wondering what we mean by $e^{{\bf A}t}$; a scalar raised to the power
        of a matrix..?  Recall that $e^{z}$ is actually defined by a convergent
        infinite sum: $$e^{z} =1 + z + \frac{1}{2} z^2 + \frac{1}{6} z^3 + ...
        .$$ The notation $e^{{\bf A}t}$ uses the same definition: $$e^{{\bf A}t}
        = {\bf I} + {\bf A}t + \frac{1}{2}({\bf A}t)^2 + \frac{1}{6}({\bf A}t)^3
        + ... .$$ Not surprisingly, this has many special forms.  For instance,
        if ${\bf A}$ is diagonalizable, $e^{{\bf A}t} = {\bf
        V}e^{{\bf\Lambda}t}{\bf V}^{-1},$ where ${\bf A} = {\bf V \Lambda
        V}^{-1}$ is the eigenvalue decomposition of ${\bf A}$
        <elib>Strang98</elib>.  The particular form we will use here is
        $$e^{-{\bf A}\tau} = \sum_{k=0}^{n-1} \alpha_k(\tau) {\bf A}^k.$$ This
        is a particularly surprising form, because the infinite sum above is
        represented by this finite sum; the derivation uses Sylvester's Theorem
        <elib>Ogata96</elib>, <elib>Chen98a</elib>.  Then we have,
        \begin{align*} \bx(0) =& - \sum_{k=0}^{n-1} {\bf A}^k \bB \int_0^{t_f}
        \alpha_k(\tau) \bu(\tau) d\tau \\ =& -\sum_{k=0}^{n-1} {\bf A}^k \bB w_k
        \text{, where } \bw_k = \int_0^{t_f} \alpha_k(\tau) \bu(\tau) d\tau \\ =& -
        \begin{bmatrix} \bB & {\bf AB} & {\bf A}^2\bB & \cdots & {\bf
        A}^{n-1}\bB \end{bmatrix}_{n \times (nm)} \begin{bmatrix} \bw_0 \\ \bw_1 \\
        \bw_2 \\ \vdots \\ \bw_{n-1} \end{bmatrix} \end{align*} The matrix
        containing the vectors $\bB$, ${\bf AB}$, ... ${\bf A}^{n-1}\bB$ is
        called the controllability matrix.  In order for the system to be
        controllable, for every initial condition $\bx(0)$ we must be able to
        find the corresponding vector ${\bf w}$.  This is only possible when the
        rows of the controllability matrix are linearly independent.  Therefore,
        the condition of controllability is that this controllability matrix is
        full rank.</p>

        <p> Although we only treated the case of a scalar $u$, it is possible to
        extend the analysis to a vector $\bu$ of size $m$, yielding the
        condition $$\text{rank} \begin{bmatrix} \bB & {\bf AB} & {\bf A}^2\bB &
        \cdots & {\bf A}^{n-1}\bB \end{bmatrix}_{n \times (nm)} = n.$$ In Matlab
        (using the Control Systems Toolbox), you can obtain the controllability
        matrix using <code>Cm = ctrb(A,B)</code>, and evaluate its rank with
        <code>rank(Cm)</code>.</p>

        <p>Note that a linear feedback to change the eigenvalues of the
        eigenmodes is not sufficient to accomplish our goal of getting to the
        goal in finite time.  In fact, the open-loop control to reach the goal
        is easily obtained with a final-value LQR problem, and (for ${\bf
        R}={\bf I}$) is actually a simple function of the controllability
        Grammian<elib>Chen98a</elib>.</p>
        </details>

        <p></p>

      </section>

      <section data-type="sect3"><h1>Controllability vs. underactuated</h1>

        <p> Analysis of the controllability of both the Acrobot and Cart-Pole
        systems reveals that the linearized dynamics about the upright are, in
        fact, controllable.  This implies that the linearized system, if started
        away from the zero state, can be returned to the zero state in finite
        time.  This is potentially surprising - after all the systems are
        underactuated.  For example, it is interesting and surprising that the
        Acrobot can balance itself in the upright position without having a
        shoulder motor.</p>

        <p> The controllability of these model systems demonstrates an extremely
        important, point: <em>An underactuated system is not necessarily an
        uncontrollable system.</em>  Underactuated systems cannot follow
        arbitrary trajectories, but that does not imply that they cannot arrive
        at arbitrary points in state space.  However, the trajectory required to
        place the system into a particular state may be arbitrarily complex.</p>

        <p> The controllability analysis presented here is for linear
        time-invariant (LTI) systems.  A comparable analysis exists for linear
        time-varying (LTV) systems.  We will even see extensions to nonlinear
        systems; although it will often be referred to by the synonym of
        "reachability" analysis.</p>

      </section>

    </section> <!-- end controllability -->

    <section data-type="sect2"><h1>LQR feedback</h1>

      <p> Controllability tells us that a trajectory to the fixed point exists,
      but does not tell us which one we should take or what control inputs cause
      it to occur.  Why not?  There are potentially infinitely many solutions.
      We have to pick one.</p>

      <p> The tools for controller design in linear systems are very advanced.
      In particular, as we describe in the <a href="?chapter=lqr">linear optimal
      control chapter</a>, one can easily design an optimal feedback controller
      for a regulation task like balancing, so long as we are willing to
      linearize the system around the operating point and define optimality in
      terms of a quadratic cost function: $$J(\bx_0) = \int_0^\infty \left[
      \bx^T(t) \bQ \bx(t) + \bu^T(t) \bR \bu(t) \right]dt, \quad \bx(0)=\bx_0,
      \bQ=\bQ^T>0, \bR=\bR^T>0.$$ The linear feedback matrix $\bK$ used as
      $$\bu(t) = - \bK\bx(t),$$ is the so-called optimal linear quadratic
      regulator (LQR).  Even without understanding the detailed derivation, we
      can quickly become practitioners of LQR. <drake></drake> provides a
      function, <code>K = LinearQuadraticRegulator(system,context,Q,R)</code>
      for linear systems.  It also provides a version <code>controller =
      LinearQuadraticRegulator(system,context,Q,R)</code> that will linearize
      the system for you around an equilibrium and return the controller (in the
      original coordinates).  Therefore, to use LQR, one simply needs to define
      the symmetric positive-definite cost matrices, ${\bf Q}$ and ${\bf R}$. In
      their most common form, ${\bf Q}$ and ${\bf R}$ are positive diagonal
      matrices, where the entries $Q_{ii}$ penalize the relative errors in state
      variable $x_i$ compared to the other state variables, and the entries
      $R_{ii}$ penalize actions in $u_i$.</p>

      <div data-type="example" class="drake"><h1>LQR for the Acrobot and Cart-Pole</h1>

      <p>Take a minute to play around with the LQR controller for the Acrobot</p>

      <pysrc>acrobot/balancing_lqr.py</pysrc>

      <p>and Cart-Pole</p>

      <pysrc>cartpole/balancing_lqr.py</pysrc>

      <p>Make sure that you take a minute to look at the code which runs during
      these examples.  Can you set the ${\bf Q}$ and ${\bf R}$ matrices
      differently, to improve the performance?</p>

      </div>

      <p> Simulation of the closed-loop response with LQR feedback shows that
      the task is indeed completed - and in an impressive manner.  Oftentimes
      the state of the system has to move violently away from the origin in
      order to ultimately reach the origin.  Further inspection reveals the
      (linearized) closed-loop dynamics are in fact non-minimum phase (acrobot
      has 3 right-half zeros, cart-pole has 1).</p>


      <div data-type="example" class="drake"><h1>LQR for a Quadrotors</h1>

      <p>LQR works essentially out of the box for Quadrotors, if linearized
      around a nominal fixed point (where the non-zero thrust from the
      propellers is balancing gravity).  We have code for the standard Quadrotor
      model in <drake></drake>, and for <a
      href="https://github.com/RobotLocomotion/drake/blob/master/examples/quadrotor/run_quadrotor_lqr.cc">stabilizing
      it with LQR</a>.  I'll include them again here as soon as I figure out how
      to distribute a 3D visualizer with the binary releases of
      <drake></drake>.</p>

      <todo>Add the LQR examples.</todo>

      </div>

      <p> Note that LQR, although it is optimal for the linearized system, is
      not necessarily the best linear control solution for maximizing basin of
      attraction of the fixed-point.  The theory of <em>robust
      control</em>(e.g., <elib>Zhou97</elib>), which explicitly takes into
      account the differences between the linearized model and the nonlinear
      model, will produce controllers which outperform our LQR solution in this
      regard.</p>

    </section>

  </section> <!-- end balancing -->

  <section data-type="sect1"><h1>Partial feedback linearization</h1>

    <p> In the introductory chapters, we made the point that the underactuated
    systems are not feedback linearizable.  At least not completely. Although we
    cannot linearize the full dynamics of the system, it is still possible to
    linearize a portion of the system dynamics.  The technique is called
    <em>partial feedback linearization</em>.</p>

    <p> Consider the cart-pole example.  The dynamics of the cart are affected
    by the motions of the pendulum.  If we know the model, then it seems quite
    reasonable to think that we could create a feedback controller which would
    push the cart in exactly the way necessary to counter-act the dynamic
    contributions from the pendulum - thereby linearizing the cart dynamics.
    What we will see, which is potentially more surprising, is that we can also
    use a feedback law for the cart to feedback linearize the dynamics of the
    passive pendulum joint.</p>

    <p> We'll use the term <em>collocated</em> partial feedback linearization to
    describe a controller which linearizes the dynamics of the actuated joints.
    What's more surprising is that it is often possible to achieve
    <em>non-collocated</em> partial feedback linearization - a controller which
    linearizes the dynamics of the unactuated joints. The treatment presented
    here follows from <elib>Spong94a</elib>.</p>

    <section data-type="sect2"><h1>PFL for the Cart-Pole System</h1>

      <section data-type="sect3"><h1>Collocated</h1>

        <p> Starting from the equations \ref{eq:simple} and \ref{eq:simple2}, we
        have

          \begin{gather*}
          \ddot\theta = -\ddot{x}c - s \\
          % \ddot\theta = -\frac{1}{l} (\ddot{x} c + g s)
          \ddot{x}(2-c^2) - sc - \dot\theta^2 s = f
          \end{gather*}
          Therefore, applying the feedback control law
          \begin{equation}
          f = (2 - c^2) \ddot{x}^d - sc - \dot\theta^2 s
          % f = (m_c + m_p) u + m_p (u c + g s) c - m_p l \dot\theta^2 s
          \end{equation}
          results in
          \begin{align*}
          \ddot{x} =& \ddot{x}^d \\ \ddot{\theta} =& -\ddot{x}^dc - s,
          \end{align*}

        which are valid globally.</p>

      </section>

      <section data-type="sect3"><h1>Non-collocated</h1>

        <p> Starting again from equations \ref{eq:simple} and \ref{eq:simple2},
        we have

        \begin{gather*}
        \ddot{x} = -\frac{\ddot\theta + s}{c} \\
        \ddot\theta(c - \frac{2}{c}) - 2 \tan\theta - \dot\theta^2 s = f
        \end{gather*}

        Applying the feedback control law

        \begin{equation}
          f = (c - \frac{2}{c}) \ddot\theta^d - 2 \tan\theta - \dot\theta^2 s
        \end{equation} results in
        \begin{align*}
          \ddot\theta =& \ddot\theta^d \\ \ddot{x} =&
          -\frac{1}{c} \ddot\theta^d - \tan\theta.
        \end{align*}

        Note that this expression is only valid when $\cos\theta \neq 0$. This
        is not surprising, as we know that the force cannot create a torque when
        the beam is perfectly horizontal.</p>

      </section>

    </section> <!-- end PFL for the cart-pole -->

    <section data-type="sect2"><h1>General form</h1>

      <p> For systems that are trivially underactuated (torques on some joints,
      no torques on other joints), we can, without loss of generality,
      reorganize the joint coordinates in any underactuated system described by
      the manipulator equations into the form:

      \begin{align}
      \bM_{11} \ddot{\bq}_1 + \bM_{12} \ddot{\bq}_2 &= \btau_1, \label{eq:passive_dyn}\\
      \bM_{21} \ddot{\bq}_1 + \bM_{22} \ddot{\bq}_2 &= \btau_2 + \bu, \label{eq:active_dyn}
      \end{align}

      with $\bq \in \Re^n$, $\bq_1 \in \Re^l$, $\bq_2 \in \Re^m$, $l=n-m$.
      $\bq_1$ represents all of the passive joints, and $\bq_2$ represents all
      of the actuated joints, and the $\btau = \btau_g - \bC\dot\bq$ terms
      capture all of the Coriolis and gravitational terms, and $$\bM(\bq) =
      \begin{bmatrix} \bM_{11} & \bM_{12} \\ \bM_{21} & \bM_{22}
      \end{bmatrix}.$$ Fortunately, because $\bM$ is uniformly (e.g. for all
      $\bq$) positive definite, $\bM_{11}$ and $\bM_{22}$ are also positive
      definite.</p>

      <section data-type="sect3"><h1>Collocated linearization</h1>

        <p> Performing the same substitutions into the full manipulator
        equations, we get:

        \begin{gather}
        \ddot\bq_1 = \bM_{11}^{-1} \left[ \btau_1 - \bM_{12} \ddot\bq_2
          \right] \\
        (\bM_{22} - \bM_{21} \bM_{11}^{-1} \bM_{12})
          \ddot\bq_2 - \btau_2 + \bM_{21} \bM_{11}^{-1} \btau_1 = \bu
        \end{gather}

        It can be easily shown that the matrix $(\bM_{22} - \bM_{21}
        \bM_{11}^{-1} \bM_{12})$ is invertible<elib>Spong94a</elib>; we can see
        from inspection that it is symmetric.  PFL follows naturally, and is
        valid globally.</p>

      </section>

      <section data-type="sect3"><h1>Non-collocated linearization</h1>

        <p>
        \begin{gather}
        \ddot\bq_2 = \bM_{12}^+ \left[ \btau_1 - \bM_{11} \ddot\bq_1
          \right] \\
        (\bM_{21} - \bM_{22} \bM_{12}^+ \bM_{11})
          \ddot\bq_1 - \btau_2 + \bM_{22} \bM_{12}^+ \btau_1 = \bu
        \end{gather}

        where $\bM_{12}^+$ is a Moore-Penrose pseudo-inverse.  This inverse
        provides a unique solution when the rank of $\bM_{12}$ equals $l$, the
        number of passive degrees of freedom in the system (it cannot be more,
        since the matrix only has $l$ rows).  This rank condition is sometimes
        called the property of "Strong Inertial Coupling".  It is state
        dependent. A system has Global Strong Inertial Coupling if it exhibits
        Strong Inertial Coupling in every state.</p>

      </section>

      <section data-type="sect3"><h1>Task-space partial feedback linearization</h1>

        <p> In general, we can define some combination of active and passive
        joints that we would like to control.  This combination is sometimes
        called a "task space". <todo>cite some task space refs here?</todo>
        Consider an output function of the form, $$\by = \bh(\bq),$$ with ${\bf
        y} \in \Re^p$, which defines the task space.  Define $\bH_1 =
        \frac{\partial \bh}{\partial \bq_1}$, $\bH_2 = \frac{\partial
        \bh}{\partial \bq_2}$, $\bH = [\bH_1,\bH_2]$.</p>

        <div data-type="theorem"><h1>Task Space PFL</h1>

        <p> If the actuated joints are commanded so that \begin{equation}
        \ddot\bq_2 = \bar\bH^+ \left [\ddot\by^d - \dot\bH\dot\bq - \bH_1
        \bM_{11}^{-1}\btau_1 \right],  \label{eq:q2cmd} \end{equation} where
        $\bar{\bH} = \bH_2 - \bH_1 \bM_{11}^{-1} \bM_{12}.$ and $\bar\bH^+$ is
        the right Moore-Penrose pseudo-inverse, $$\bar\bH^+ = \bar\bH^T (\bar\bH
        \bar\bH^T)^{-1},$$ then we have \begin{equation} \ddot\by =
        \ddot\by^d.\end{equation} subject to \begin{equation}\text{rank}\left(\bar{\bH}
        \right) = p, \label{eq:rank_condition}\end{equation} </p>

        <p><b>Proof Sketch.</b>
        Differentiating the output function we have
        \begin{gather*}
        \dot\by = \bH \dot\bq \\
        \ddot\by = \dot\bH \dot\bq + \bH_1 \ddot\bq_1 + \bH_2 \ddot\bq_2.
        \end{gather*}
        Solving \ref{eq:passive_dyn} for the dynamics of the unactuated joints
        we have:
        \begin{equation}
        \ddot\bq_1 = \bM_{11}^{-1} (\btau_1 - \bM_{12} \ddot\bq_2) \label{eq:q1cmd}
        \end{equation}
        Substituting, we have
        \begin{align}
        \ddot\by =& \dot\bH \dot\bq + \bH_1 \bM_{11}^{-1}(\btau_1 -
          \bM_{12}\ddot\bq_2) + \bH_2 \ddot\bq_2 \\
        =& \dot\bH \dot\bq + \bar{\bH} \ddot\bq_2 + \bH_1 \bM_{11}^{-1}\btau_1 \\
        =& \ddot\by^d
        \end{align}
        Note that the last line required the rank condition
        ($\ref{eq:rank_condition}$) on $\bar\bH$ to ensure that the rows
        of $\bar{\bH}$ are linearly independent, allowing $\bar\bH
        \bar\bH^+ = \bI$.</p>

        </div>

        <p> In order to execute a task space trajectory one could command
        $$\ddot\by^d = \ddot{\bar{\by}}^d + \bK_d (\dot{\bar{\by}}^d - \dot\by) +
        \bK_p (\bar{\by}^d -\by).$$ Assuming the internal dynamics are stable,
        this yields converging error dynamics, $(\bar{\by}^d - \by)$, when
        $\bK_p,\bK_d > 0$<elib>Slotine90</elib>.  For a position control robot,
        the acceleration command of ($\ref{eq:q2cmd}$) suffices. Alternatively,
        a torque command follows by substituting ($\ref{eq:q2cmd}$) and
        ($\ref{eq:q1cmd}$) into ($\ref{eq:active_dyn}$).</p>

        <div data-type="example"><h1>End-point trajectory following with the Cart-Pole system</h1>

        <p> Consider the task of trying to track a desired kinematic trajectory
        with the endpoint of pendulum in the cart-pole system.  With one
        actuator and kinematic constraints, we might be hard-pressed to track a
        trajectory in both the horizontal and vertical coordinates.  But we can
        at least try to track a trajectory in the vertical position of the
        end-effector.</p>

        <p>Using the task-space PFL derivation, we have:
        \begin{gather*}
        y = h(\bq) = -l \cos\theta \\
        \dot{y} = l \dot\theta \sin\theta
        \end{gather*}
        If we define a desired trajectory:
        $$\bar{y}^d(t) = \frac{l}{2} + \frac{l}{4} \sin(t),$$
        then the task-space controller is easily implemented using the
        derivation above.</p>

        <todo>add results here</todo>
        </div> <!-- end example-->

        <p> The task space derivation above provides a convenient generalization
        of the partial feedback linearization (PFL) <elib>Spong94a</elib>, which
        encompasses both the collocated and non-collocated results.  If we
        choose $\by = \bq_2$ (collocated), then we have $$\bH_1 = 0, \bH_2 =
        \bI, \dot\bH = 0, \bar\bH = \bI, \bar\bH^+ = \bI.$$ From this, the
        command in ($\ref{eq:q2cmd}$) reduces to $\ddot\bq_2 = \ddot\bq_2^d$.
        The actuator command is then $$ \bu = \bM_{21} \bM_{11}^{-1} (\btau_1 -
        \bM_{12} \ddot\bq_2^d) + \bM_{22} \ddot\bq_2^d + \btau_2,$$ and the rank
        condition ($\ref{eq:rank_condition}$) is always met.</p>

        <p> If we choose ${\bf y} = \bq_1$ (non-collocated), we have $$\bH_1 =
        \bI, \bH_2 = 0, \dot\bH = 0, \bar{\bH}=-\bM_{11}^{-1}\bM_{12}.$$ The
        rank condition ($\ref{eq:rank_condition}$) requires that
        $\text{rank}(\bM_{12}) = l$, in which case we can write
        $\bar{\bH}^+=-\bM_{12}^+\bM_{11}$, reducing the rank condition to
        precisely the "Strong Inertial Coupling" condition described in
        <elib>Spong94a</elib>. Now the command in ($\ref{eq:q2cmd}$) reduces to
        \begin{equation} \ddot\bq_2 = \bM_{12}^+ \left [ \btau_1 -
        \bM_{11}\ddot\bq_1^d \right] \label{eq:q2ddnonco} \end{equation} The
        actuator command is found by substituting ($\ref{eq:q2ddnonco}$) into
        ($\ref{eq:active_dyn}$), yielding the same results as in
        <elib>Spong94a</elib>.</p>

      </section> <!-- task space PFL -->
    </section>
  </section>

  <section data-type="sect1"><h1>Swing-up control</h1>

    <section data-type="sect2"><h1>Energy shaping</h1>

      <p> Recall the phase portraits that we used to understand the dynamics of
      the undamped, unactuated, simple pendulum ($u=b=0$).  The orbits of this
      phase plot were defined by contours of constant energy.  One very special
      orbit, known as a <em>homoclinic</em> orbit, is the orbit which passes
      through the unstable fixed point.  In fact, visual inspection will reveal
      that any state that lies on this homoclinic orbit must pass into the
      unstable fixed point.  Therefore, if we seek to design a nonlinear
      feedback control policy which drives the simple pendulum from any initial
      condition to the unstable fixed point, a very reasonable strategy would be
      to use actuation to regulate the energy of the pendulum to place it on
      this homoclinic orbit, then allow the system dynamics to carry us to the
      unstable fixed point.</p>

      <p> This idea turns out to be a bit more general than just for the simple
      pendulum. As we will see, we can use similar concepts of `energy shaping'
      to produce swing-up controllers for the acrobot and cart-pole systems.
      It's important to note that it only takes one actuator to change the total
      energy of a system.</p>

      <p> Although a large variety of swing-up controllers have been proposed
      for these model systems (c.f. <elib>Fantoni02</elib>,
      <elib>Araki05</elib>, <elib>Xin04</elib>, <elib>Spong94</elib>,
      <elib>Mahindrakar05</elib>, <elib>Berkemeier99</elib>,
      <elib>Murray91</elib>, <elib>Lai06</elib>), the energy shaping controllers
      tend to be the most natural to derive and perhaps the most well-known.</p>

    </section>

    <section data-type="sect2" id="s:pend_energy_shaping"><h1>Simple pendulum</h1>

      <p> Recall the equations of motion for the undamped simple pendulum were
      given by $$ml^2 \ddot\theta + mgl\sin\theta = u.$$  The total energy of
      the simple pendulum is given by $$E = \frac{1}{2} m l^2 \dot\theta^2 -
      mgl\cos\theta.$$  To understand how to control the energy, observe that
      \begin{align*} \dot{E} =& ml^2\dot\theta \ddot\theta + \dot\theta
      mgl\sin\theta \\ =& \dot\theta \left[ u - mgl\sin\theta \right] +
      \dot\theta mgl\sin\theta \\ =& u\dot\theta. \end{align*} In words, adding
      energy to the system is simple - simply apply torque in the same direction
      as $\dot\theta$.  To remove energy, simply apply torque in the opposite
      direction (e.g., damping).</p>

      <p> To drive the system to the homoclinic orbit, we must regulate the
      energy of the system to a particular desired energy, $$E^d = mgl.$$ If we
      define $\tilde{E} = E - E^d$, then we have $$\dot{\tilde{E}} = \dot{E} =
      u\dot\theta.$$ If we apply a feedback controller of the form $$u = -k
      \dot\theta \tilde{E},\quad k>0,$$ then the resulting error dynamics are
      $$\dot{\tilde{E}} = - k \dot\theta^2 \tilde{E}.$$ These error dynamics
      imply an exponential convergence: $$\tilde{E} \rightarrow 0,$$ except for
      states where $\dot\theta=0$.  The essential property is that when $E >
      E^d$, we should remove energy from the system (damping) and when $E <
      E^d$, we should add energy (negative damping).  Even if the control
      actions are bounded, the convergence is easily preserved.</p>

      <p> This is a nonlinear controller that will push all system trajectories
      to the unstable equilibrium.  But does it make the unstable equilibrium
      locally stable?  <em>It is asymptotically stable, but not stable
      i.s.L.</em>  Small perturbations may cause the system to drive all of the
      way around the circle in order to once again return to the unstable
      equilibrium.  For this reason, once trajectories come into the vicinity of
      our swing-up controller, we prefer to switch to our LQR balancing
      controller to performance to complete the task.</p>

      <div data-type="example" class="drake"><h1>Energy Shaping for the
      Pendulum</h1>

      <p>Take a minute to play around with the energy shaping + LQR controller
      for swinging up the pendulum</p>

      <pysrc>pendulum/energy_swingup_and_balance.py</pysrc>

      <p>Make sure that you take a minute to look at the code which runs during
      these examples.  Note the somewhat arbitrary threshold for switching to
      the LQR controller.  We'll give a much more satisfying answer for that in
      the <a href="?chapter=lyapunov">chapter on Lyapunov methods</a>.</p>

      </div>

    </section>

    <section data-type="sect2"><h1>Cart-Pole</h1>

      <p> Having thought about the swing-up problem for the simple pendulum,
      let's try to apply the same ideas to the cart-pole system.  The basic
      idea, from <elib>Chung95</elib>, is to use collocated PFL to simplify the
      dynamics, use energy shaping to regulate the pendulum to its homoclinic
      orbit, then to add a few terms to make sure that the cart stays near the
      origin.  The collocated PFL (when all parameters are set to 1) left us
      with:

      \begin{gather}
      \ddot{x} = u \\
      \ddot\theta = - u c - s
      %\ddot\theta = -\frac{1}{l}( u c + g s )
      \end{gather}

      The energy <em>of the pendulum</em> (a unit mass, unit length, simple
      pendulum in unit gravity) is given by: $$E(\bx) = \frac{1}{2} \dot\theta^2
      - \cos\theta.$$ <!-- E = \frac{1}{2}ml^2\dot\theta^2 - mgl\cos\theta -->
      The desired energy, equivalent to the energy at the desired fixed-point,
      is $$E^d = 1.$$ <!--  E^d = m_p g l --> Again defining $\tilde{E}(\bx) =
      E(\bx) - E^d$, we now observe that

      \begin{align*}
      \dot{\tilde{E}}(\bx) =& \dot{E}(\bx) = \dot\theta \ddot\theta +
      \dot\theta s \\
      % \dot\tilde{E} = ml^2 \dot\theta \ddot\theta + mgl s
      =& \dot\theta [ -uc - s] + \dot\theta s \\
      % - ml \dot\theta [ u c + g s ] + mgl s
      =& - u\dot\theta \cos\theta.
      % - ml u \dot\theta c
      \end{align*}

      Therefore, if we design a controller of the form $$u = k
      \dot\theta\cos\theta \tilde{E},\quad k>0$$ the result is $$\dot{\tilde{E}}
      = - k \dot\theta^2 \cos^2\theta \tilde{E}.$$  <!-- times ml --> This
      guarantees that $| \tilde{E} |$ is non-increasing, but isn't quite enough
      to guarantee that it will go to zero.  For example, if $\theta =
      \dot\theta = 0$, the system will never move.  However, if we have that
      $$\int_0^t \dot\theta^2(t') \cos^2 \theta(t') dt' \rightarrow \infty,\quad
      \text{as } t \rightarrow \infty,$$ then we have $\tilde{E}(t) \rightarrow
      0$. This condition is satisfied for all but the trivial constant
      trajectories at fixed points.</p>

      <p> Now we return to the full system dynamics (which includes the cart).
      <elib>Chung95</elib> and <elib>Spong96</elib> use the simple pendulum
      energy controller with an addition PD controller designed to regulate the
      cart: $$\ddot{x}^d = k_E \dot\theta \cos\theta \tilde{E} - k_p x - k_d
      \dot{x}.$$ <elib>Chung95</elib> provides a proof of convergence for this
      controller with some nominal parameters.  <elib>Fantoni02</elib> provides
      a slightly different controller derived directly from a Lyapunov
      argument.</p>

      <figure>
      <img width="70%" data-src="figures/cartpole_swingup_phase.svg"/>

      <figcaption>Cart-Pole Swingup: Example phase plot of the pendulum
      subsystem using energy shaping control.  The controller drives the system
      to the homoclinic orbit, then switches to an LQR balancing controller near
      the top.</figcaption>
      </figure>

    </section>

    <section data-type="sect2"><h1>Acrobot</h1>

      <p> Swing-up control for the acrobot can be accomplished in much the same
      way.  <elib>Spong94</elib> - pump energy.  Clean and simple.  No proof.
      Slightly modified version (uses arctan instead of sat) in
      <elib>Spong95</elib>. Clearest presentation in <elib>Spong96</elib>.</p>

      <p> Use collocated PFL.  ($\ddot{q}_2 = \ddot{q}_2^d$). $$E(\bx) =
      \frac{1}{2}\dot\bq^T\bM\dot{\bq} + U(\bx).$$ $$ E_d = U(\bx^*).$$
      $$\bar{u} = \dot{q}_1 \tilde{E}.$$ $$\ddot{q}_2^d  = - k_1 q_2 - k_2
      \dot{q}_2 + k_3 \bar{u},$$</p>

      <p> Extra PD terms prevent proof of asymptotic convergence to homoclinic
      orbit.  Proof of another energy-based controller in
      <elib>Xin04</elib>.</p>

    </section>

    <section data-type="sect2"><h1>Discussion</h1>

      <p> The energy shaping controller for swing-up presented here is a pretty
      faithful representative from the field of nonlinear underactuated control.
      Typically these control derivations require some clever tricks for
      simplifying or canceling out terms in the nonlinear equations, then some
      clever Lyapunov function to prove stability.  In these cases, PFL was used
      to simplify the equations, and therefore the controller design.</p>

      <p> These controllers are important, representative, and relevant.  But
      clever tricks with nonlinear equations seem to be fundamentally limited.
      Most of the rest of the material presented in this book will emphasize
      more general computational approaches to formulating and solving these and
      other control problems. </p>

    </section>

  </section>

  <section data-type="sect1"><h1>Other model systems</h1>

    <p> The acrobot and cart-pole systems are just two of the model systems used
    heavily in underactuated control research.  Other examples include:</p> <ul>
    <li>Pendubot</li> <li>Inertia wheel pendulum</li> <li>Furuta pendulum
    (horizontal rotation and vertical pendulum)</li> <li>Hovercraft</li> </ul>


  </section>

</section> <!-- ***************  end of acrobot/cartpole/quadrotor chapter **************   -->


<section data-type="chapter" class="chapter" id="simple_legs"><h1>Walking and
  Running</h1>

  <p>Practical legged locomotion is one of the fundamental unsolved problems in
  robotics.  Many challenges are in mechanical design - a walking robot must
  carry all of its actuators and power, making it difficult to carry ideal
  force/torque - controlled actuators.  But many of the unsolved problems are
  because walking robots are underactuated control systems. </p>

  <p>In this chapter we'll introduce some of the simple models of walking and
  robots, the control problems that result, and a very brief summary of some of
  the control solutions described in the literature.  Compared to the robots
  that we have studied so far, our investigations of legged locomotion will
  require additional tools for thinking about limit cycle dynamics and dealing
  with impacts.</p>

  <section data-type="sect1"><h1>Why is walking so hard?</h1>

  </section> <!-- walking is hard -->

  <section data-type="sect1"><h1>State of the art</h1>

  </section> <!-- state of the art -->

  <section data-type="sect1"><h1>The Ballistic Walker</h1>

    <p>One of the earliest models of walking was proposed by
    McMahon<elib>McMahon80</elib>, who argued that humans use a mostly ballistic
    (passive) gait.  COM trajectory looks like a pendulum (roughly walking by
    vaulting).  EMG activity in stance legs is high, but EMG in swing leg is
    very low, except for very beginning and end of swing.  Proposed a three-link
    "ballistic walker" model, which models a single swing phase (but not
    transitions to the next swing nor stability). Interestingly, in recent years
    the field has developed a considerably deeper appreciation for the role of
    compliance during walking; simple walking-by-vaulting models are starting to
    fall out of favor.</p>

    <p>McGeer<elib>McGeer90</elib> followed up with a series of walking
    machines, called "passive dynamic walkers".  The walking machine by Collins
    and Ruina<elib>Collins01</elib> is the most impressive passive walker to
    date.</p>

  </section> <!-- end of ballistic walker -->

  <section data-type="sect1"><h1>The Rimless Wheel</h1>

    <figure>
    <img width="40%" data-src="figures/rimlessWheel.svg"/>
    <figcaption>The rimless wheel. The orientation of the stance leg,
    $\theta$, is measured clockwise from the vertical axis. </figcaption>
    </figure>

    <p>The most elementary model of passive dynamic walking, first used in the
    context of walking by <elib>McGeer90</elib>, is the rimless wheel.  This
    simplified system has rigid legs and only a point mass at the hip as
    illustrated in the figure above.  To further simplify the analysis, we make
    the following modeling assumptions:</p> <ul> <li> Collisions with ground are
    inelastic and impulsive (only angular momentum is conserved around the point
    of collision).</li> <li> The stance foot acts as a pin joint and does not
    slip.</li> <li> The transfer of support at the time of contact is
    instantaneous (no double support phase).</li> <li> $0 \le \gamma <
    \frac{\pi}{2}$, $0 < \alpha < \frac{\pi}{2}$, $l > 0$.</li> </ul> <p>Note
    that the coordinate system used here is slightly different than for the
    simple pendulum ($\theta=0$ is at the top, and the sign of $\theta$ has
    changed).</p> <todo> update the coordinates (here and in drake)</todo>

    <p> The most comprehensive analysis of the rimless wheel was done by
    <elib>Coleman98a</elib>.</p>

    <section data-type="sect2"><h1>Stance Dynamics</h1>

      <p>The dynamics of the system when one leg is on the ground are given by
      $$\ddot\theta = \frac{g}{l}\sin(\theta).$$ If we assume that the system is
      started in a configuration directly after a transfer of support
      ($\theta(0^+) = \gamma-\alpha$), then forward walking occurs when the
      system has an initial velocity, $\dot\theta(0^+) > \omega_1$, where
      $$\omega_1 = \sqrt{ 2 \frac{g}{l} \left[ 1 - \cos \left (\gamma-\alpha
      \right) \right]}.$$ $\omega_1$ is the threshold at which the system has
      enough kinetic energy to vault the mass over the stance leg and take a
      step.  This threshold is zero for $\gamma = \alpha$ and does not exist for
      $\gamma > \alpha$.  The next foot touches down when $\theta(t) =
      \gamma+\alpha$, at which point the conversion of potential energy into
      kinetic energy yields the velocity $$\dot\theta(t^-) = \sqrt{
      \dot\theta^2(0^+) + 4\frac{g}{l} \sin\alpha \sin\gamma }.$$ $t^-$ denotes
      the time immediately before the collision.</p>

    </section>

    <section data-type="sect2"><h1>Foot Collision</h1>

      <figure> <img width="50%" data-src="figures/rimlessWheel_collision.svg"/>
      <figcaption>Angular momentum is conserved around the point of
      impact</figcaption> </figure>

      <p> The angular momentum around the point of collision at time $t$ just
      before the next foot collides with the ground is $$L(t^-) =
      -ml^2\dot\theta(t^-) \cos(2\alpha).$$ The angular momentum at the same
      point immediately after the collision is $$L(t^+) =
      -ml^2\dot\theta(t^+).$$ Assuming angular momentum is conserved, this
      collision causes an instantaneous loss of velocity: $$\dot\theta(t^+) =
      \dot\theta(t^-) \cos(2\alpha).$$</p>

    </section>

    <section data-type="sect2"><h1>Forward simulation</h1>

      <p>Given the stance dynamics, the collision detection logic ($\theta =
      \gamma \pm \alpha$), and the collision update, we can numerically simulate
      this simple model.  Doing so reveals something remarkable... the wheel
      starts rolling, but then one of two things happens: it either runs out of
      energy and stands still, or it quickly falls into a stable periodic
      solution.  Convergence to this stable periodic solution appears to happen
      from a huge range of initial conditions.  Try it yourself.</p>

      <div data-type="example" class="drake"><h1>Numerical simulation of the
      rimless wheel</h1>

        <pysrc>rimless_wheel/simulate.py</pysrc>

        <p>Optionally you can specify the initial conditions.  Here are a few
        good values to try:</p>

        <pysrc args="--initial_angular_velocity=5.0">rimless_wheel/simulate.py</pysrc>
        <pysrc args="--initial_angular_velocity=10.0">rimless_wheel/simulate.py</pysrc>
        <pysrc args="--initial_angular_velocity=0.95">rimless_wheel/simulate.py</pysrc>
        <pysrc args="--initial_angular_velocity=-5.0">rimless_wheel/simulate.py</pysrc>
        <pysrc args="--initial_angular_velocity=-4.8">rimless_wheel/simulate.py</pysrc>

      </div>

      <p>One of the fantastic things about the rimless wheel is that the
      dynamics are exactly the undamped simple pendulum that we've thought so
      much about.  So you will recognize the phase portraits of the system below
      - they are centered around the unstable fixed point of the simple
      pendulum.  Phase plots of the trajectories from the various initial
      conditions recommended above are plotted below.</p>

      <figure>
       <img width="80%" data-src="figures/rimlessWheel_phase1.svg"/>
       <img width="80%" data-src="figures/rimlessWheel_phase2.svg"/>
       <img width="80%" data-src="figures/rimlessWheel_phase3.svg"/>
       <img width="80%" data-src="figures/rimlessWheel_phase4.svg"/>
       <figcaption>Phase portrait trajectories of the rimless wheel
       ($m=1$, $l=1$, $g=9.8$, $\alpha=\pi/8$, $\gamma=0.08$).</figcaption>
      </figure>

      <p>Take a minute to make sure you can follow these trajectories through
      state space for each of the initial conditions.  It's non-trivial, but
      worth the effort.</p>

    </section> <!-- end of forward simulation -->

  </section> <!-- end rimless wheel -->

  <section data-type="sect1"><h1>Limit Cycles</h1>

    <p> A limit cycle is an asymptotically stable or unstable periodic
    orbit&dagger;<sidenote>&dagger; Marginally-stable orbits, such as the
    closed-orbits of the undamped simple pendulum, are typically not called
    limit cycles.</sidenote>.  One of the simplest models of limit cycle
    behavior is the Van der Pol oscillator.  Let's examine that first...</p>

    <div data-type="example"><h1>Van der Pol Oscillator</h1>

      <p>$$\ddot{q} + \mu (q^2 - 1)\dot{q} + q = 0, \quad \mu>0$$ One can think
      of this system as almost a simple spring-mass-damper system, except that
      it has nonlinear damping.  In particular, the velocity term dissipates
      energy when $|q|>1$, and adds energy when $|q|<1$.  Therefore, it is not
      terribly surprising to see that the system settles into a stable
      oscillation from almost any initial conditions (the exception is the state
      $q=0,\dot{q}=0$).  This can be seen nicely in the phase portrait
      below.</p>

      <figure>
      <table width="100%"><tr><td width="50%">
        <img width="100%" data-src="figures/vanderPol_phase.svg"/>
      </td><td width="50%">
        <img width="100%" data-src="figures/vanderPol_time.svg"/>
      </td></tr></table>
      <figcaption>System trajectories of the Van der Pol oscillator with $\mu
        =.2$. (Left) phase portrait.  (Right) time domain.</figcaption>
      </figure>

      <p>However, as you can see from the plot of system trajectories in the
      time domain, then a slightly different picture emerges. Although the phase
      portrait clearly reveals that all trajectories converge to the same orbit,
      the time domain plot reveals that these trajectories do not necessarily
      synchronize in time.</p>

    </div> <!-- end of VdP example -->

    <p>The Van der Pol oscillator clearly demonstrates what we would think of as
    a stable limit cycle, but also exposes the subtlety in defining this limit
    cycle stability.  Neighboring trajectories do not necessarily converge on a
    stable limit cycle.  In contrast, defining the stability of a particular
    trajectory (parameterized by time) is relatively easy.</p>

    <p>Let's make a few quick points about the existence of closed-orbits. If
    we can define a closed region of phase space which does not contain any
    fixed points, then it must contain a closed-orbit<elib>Strogatz94</elib>.
    By closed, I mean that any trajectory which enters the region will stay in
    the region (this is the Poincar&eacute-Bendixson Theorem).  It's also
    interesting to note that gradient potential fields (e.g. Lyapunov functions)
    cannot have a closed-orbit<elib>Strogatz94</elib>, and consequently Lyapunov
    analysis cannot be applied to limit cycle stability without some
    modification.</p>

  </section>

  <section data-type="sect1"><h1>Poincar&eacute; Maps</h1>

    <p> One definition for the stability of a limit cycle uses the method of
    Poincar&eacute.  Let's consider an $n$ dimensional dynamical system,
    $\dot{\bx} = {\bf f}(\bx).$ Define an $n-1$ dimensional <em>surface of
    section</em>, $S$.  We will also require that $S$ is transverse to the flow
    (i.e., all trajectories starting on $S$ flow through $S$, not parallel to
    it).  The Poincar&eacute map (or return map) is a mapping from $S$ to
    itself:  $$\bx_p[n+1] = {\bf P}(\bx_p[n]),$$ where $\bx_p[n]$ is the state
    of the system at the $n$th crossing of the surface of section. Note that we
    will use the notation $\bx_p$ to distinguish the state of the discrete-time
    system from the continuous time system; they are related by $\bx_p[n] =
    \bx(t_c[n])$, where $t_c[n]$ is the time of the $n$th crossing of $S$.</p>

    <div data-type="example"><h1>Return map for the Van der Pol Oscillator</h1>

      <p>Since the full system is two dimensional, the return map dynamics are
      one dimensional.  One dimensional maps, like one dimensional flows, are
      amenable to graphical analysis.  To define a Poincar&eacute section for
      the Van der Pol oscillator, let $S$ be the line segment where $q = 0$,
      $\dot{q} > 0$.</p>

      <figure> <table><tr><td> <img width="100%"
      data-src="figures/vanderPol_section.svg" /> </td><td> <img width="100%"
      data-src="figures/vanderPol_retmap.svg" /> </td></tr></table>
      <figcaption>(Left) Phase plot with the surface of section, $S$, drawn with
      a black dashed line.  (Right) The resulting Poincar&eacute first-return
      map (blue), and the line of slope one (red).</figcaption> </figure>

    </div>

    <p>If ${\bf P}(\bx_p)$ exists for all $\bx_p \in S$, then this method turns
    the stability analysis for a limit cycle into the stability analysis of a
    fixed point on a discrete map.  In practice it is often difficult or
    impossible to find ${\bf P}$ analytically, but it can be obtained quite
    reasonably numerically.  Once ${\bf P}$ is obtained, we can infer local
    limit cycle stability with an eigenvalue analysis.  There will always be a
    single eigenvalue of 0 - corresponding to perturbations along the limit
    cycle which do not change the state of first return. The limit cycle is
    considered locally exponentially stable if all remaining eigenvalues,
    $\lambda_i$, have magnitude less than one, $|\lambda_i|<1$.</p>

    <p>In fact, it is often possible to infer more global stability properties
    of the return map by examining ${\bf P}$. <elib>Koditschek91</elib>
    describes some of the stability properties known for <em>unimodal</em>
    maps.</p>

    <p>A particularly graphical method for understanding the dynamics of a
    one-dimensional iterated map is with the staircase method.  Sketch the
    Poincar&eacute map and also the line of slope one.  Fixed points are the
    crossings with the unity line.  Asymptotically stable if $|\lambda| < 1$.
    Unlike one dimensional flows, one dimensional maps can have oscillations
    (happens whenever $\lambda < 0$).</p>

    <todo>[insert staircase diagram of van der Pol oscillator return map
    here]</todo>

  </section> <!-- end of Poincare -->

  <section data-type="sect1"><h1>Analysis of the Rimless Wheel</h1>

    <section data-type="sect2"><h1>Poincar&eacute Map</h1>

      <p>We can now derive the angular velocity at the beginning of each stance
      phase as a function of the angular velocity of the previous stance phase.
      First, we will handle the case where $\gamma \le \alpha$ and
      $\dot\theta_n^+ > \omega_1$.  The "step-to-step return map", factoring
      losses from a single collision, the resulting map is:
      $$\dot\theta^{+}_{n+1} = \cos(2\alpha) \sqrt{ ({\dot\theta_n}^{+})^{2} +
      4\frac{g}{l}\sin\alpha \sin\gamma}.$$ where the $\dot{\theta}^{+}$
      indicates the velocity just <em>after</em> the energy loss at impact has
      occurred.</p>

      <p>Using the same analysis for the remaining cases, we can complete the
      return map.  The threshold for taking a step in the opposite direction is
      $$\omega_2 = - \sqrt{2 \frac{g}{l} [1 - \cos(\alpha + \gamma)]}.$$ For
      $\omega_2 < \dot\theta_n^{+} < \omega_1,$ we have $$\dot\theta_{n+1}^{+} =
      -\dot\theta_n^{+} \cos(2\alpha).$$ Finally, for $\dot\theta_n^{+} <
      \omega_2$, we have $$\dot\theta_{n+1}^{+} = -
      \cos(2\alpha)\sqrt{(\dot\theta_n^{+})^2 - 4 \frac{g}{l} \sin\alpha
      \sin\gamma}.$$  Altogether, we have (for $\gamma \le \alpha$)
      $$\dot\theta_{n+1}^{+} = \begin{cases} \cos(2\alpha)
      \sqrt{(\dot\theta_n^{+})^2 + 4 \frac{g}{l} \sin\alpha \sin\gamma}, &
      \text{ } \omega_1 < \dot\theta_n^{+} \\ -\dot\theta_n^{+} \cos(2\alpha), &
      \text{ } \omega_2 < \dot\theta_n^{+} < \omega_1 \\ -\cos(2\alpha)
      \sqrt{(\dot\theta_n^{+})^2 - 4\frac{g}{l} \sin\alpha \sin\gamma}, &
      \dot\theta_n^{+} < w_2 \end{cases}.$$ </p>

      <p>Notice that the return map is undefined for $\dot\theta_n = \{
      \omega_1, \omega_2 \}$, because from these configurations, the wheel will
      end up in the (unstable) equilibrium point where $\theta = 0$ and
      $\dot\theta = 0$, and will therefore never return to the map.</p>

      <p>This return map blends smoothly into the case where $\gamma > \alpha$.
      In this regime, $$\dot\theta_{n+1}^{+} = \begin{cases} \cos(2\alpha)
      \sqrt{(\dot\theta_n^{+})^2 + 4 \frac{g}{l} \sin\alpha \sin\gamma}, &
      \text{ } 0 \le \dot\theta_n^{+} \\ -\dot\theta_n^{+} \cos(2\alpha), &
      \text{ } \omega_2 < \dot\theta_n^{+} < 0 \\ -\cos(2\alpha)
      \sqrt{(\dot\theta_n^{+})^2 - 4\frac{g}{l} \sin\alpha \sin\gamma}, &
      \dot\theta_n^{+} \le w_2 \end{cases}.$$ Notice that the formerly undefined
      points at $\{\omega_1,\omega_2\}$ are now well-defined transitions with
      $\omega_1 = 0$, because it is kinematically impossible to have the wheel
      statically balancing on a single leg.</p>

    </section> <!-- poincare map -->

    <section data-type="sect2"><h1>Fixed Points and Stability</h1>

      <p>For a fixed point, we require that $\dot\theta_{n+1}^{+} =
      \dot\theta_n^{+} = \omega^*$.  Our system has two possible fixed points,
      depending on the parameters: $$ \omega_{stand}^* = 0,~~~ \omega_{roll}^* =
      \cot(2 \alpha) \sqrt{4 \frac{g}{l} \sin\alpha\sin\gamma}.$$ The limit
      cycle plotted above illustrates a state-space trajectory in the vicinity
      of the rolling fixed point. $\omega_{stand}^*$ is a fixed point whenever
      $\gamma < \alpha$. $\omega_{roll}^*$ is a fixed point whenever
      $\omega_{roll}^* > \omega_1$.  It is interesting to view these
      bifurcations in terms of $\gamma$.  For small $\gamma$, $\omega_{stand}$
      is the only fixed point, because energy lost from collisions with the
      ground is not compensated for by gravity.  As we increase $\gamma$, we
      obtain a stable rolling solution, where the collisions with the ground
      exactly balance the conversion of gravitational potential to kinetic
      energy. As we increase $\gamma$ further to $\gamma > \alpha$, it becomes
      impossible for the center of mass of the wheel to be inside the support
      polygon, making standing an unstable configuration.</p>

      <figure> <img width="100%" data-src="figures/rw_retmap.svg"/>
      <figcaption>Limit cycle trajectory of the rimless wheel
      ($m=1,l=1,g=9.8,\alpha=\pi/8,\gamma=0.15$).  All hatched regions converge
      to the rolling fixed point, $\omega_{roll}^*$; the white regions converge
      to zero velocity ($\omega_{stand}^*$).</figcaption> </figure>

    </section>

  </section> <!-- end RW analysis -->

  <section data-type="sect1"><h1>The Compass Gait</h1>

    <p>The rimless wheel models only the dynamics of the stance leg, and simply
    assumes that there will always be a swing leg in position at the time of
    collision.  To remove this assumption, we take away all but two of the
    spokes, and place a pin joint at the hip.  To model the dynamics of swing,
    we add point masses to each of the legs.  For actuation, we first consider
    the case where there is a torque source at the hip - resulting in swing
    dynamics equivalent to an Acrobot (although in a different coordinate
    frame).</p>

    <figure>
    <img width="45%" data-src="figures/compass_gait.svg"/>
    <figcaption>The compass gait</figcaption>
    </figure>

    <p>In addition to the modeling assumptions used for the rimless wheel, we
    also assume that the swing leg retracts in order to clear the ground without
    disturbing the position of the mass of that leg. This model, known as the
    compass gait, is well studied in the literature using numerical methods
    <elib>Goswami96a+Goswami99+Spong03</elib>, but relatively little is known about it
    analytically.</p>

    <p>The state of this robot can be described by 4 variables:
    $\theta_{st},\theta_{sw},\dot\theta_{st}$, and $\dot\theta_{sw}$. The
    abbreviation $st$ is shorthand for the stance leg and $sw$ for the swing
    leg. Using $\bq = [ \theta_{st}, \theta_{sw} ]^T$ and $\bu = \tau$, we can
    write the dynamics as $$\bM(\bq)\ddot\bq + \bC(\bq,\dot\bq)\dot\bq =
    \btau_g(q) + \bB\bu,$$ with \begin{gather*} \bM = \begin{bmatrix} (m_h+m)l^2
    + ma^2 & -mlb\cos(\theta_{st}-\theta_{sw}) \\
    -mlb\cos(\theta_{st}-\theta_{sw}) & mb^2 \end{bmatrix}\\ \bC =
    \begin{bmatrix} 0 & -mlb\sin(\theta_{st}-\theta_{sw})\dot\theta_{sw} \\
    mlb\sin(\theta_{st}-\theta_{sw})\dot\theta_{st} & 0 \end{bmatrix} \\
    \btau_g(q) = \begin{bmatrix} (m_hl + ma + ml)g\sin(\theta_{st}) \\
    -mbg\sin(\theta_{sw}) \end{bmatrix},\\ \bB = \begin{bmatrix} -1 \\ 1
    \end{bmatrix} \end{gather*} and $l=a+b$.</p>

    <!-- My derivation is here: https://www.wolframcloud.com/objects/02914093-8fcd-4bd2-abb3-e76ca196b393 -->

    <p>The foot collision is an instantaneous change of velocity caused by a
    impulsive force at the foot that brings the foot to rest.  The update to the
    velocities can be calculated using the derivation for impulsive collisions
    derived in the appendix.  To use it, we proceed with the following
    steps:</p>

    <ul> <li>Add a "floating base" to the system by adding a free (x,y) joint at
    the pre-impact stance toe, $\bq_{fb} =
    [x,y,\theta_{st},\theta_{sw}]^T.$</li> <li>Calculate the mass matrix for
    this new system, call it $\bM_{fb}$.</li> <li>Write the position of the
    (pre-impact) swing toe as a function $\bphi(\bq_{fb})$.  Define the Jacobian
    of this function: $\bJ = \frac{\partial \bphi}{\partial \bq_{fb}}.$</li>
    <li>The post-impact velocity that ensures that $\dot\bphi = 0$ after the
    collision is given by $$\dot\bq^+ = \left[ \bI -
    \bM_{fb}^{-1}\bJ^T[\bJ\bM_{fb}^{-1}\bJ^T]^{-1}\bJ\right] \dot\bq^-,$$ noting
    that $\dot{x}^- = \dot{y}^- = 0$, and that you need only read out the last
    two elements of $\dot{\bq}^+$.  The velocity of the post-impact stance foot
    will be zero by definition, and the new velocity of the pre-impact stance
    foot can be completely determined from the minimal coordinates.</li>
    <li>Switch the stance and swing leg positions and velocities.</li> </ul>

    <!-- My derivation is here: https://www.wolframcloud.com/objects/60e3d6c1-156f-4604-bf70-1e374f471407 -->

    <div data-type="example" class="drake"><h1>Numerical simulation of the
      compass gait</h1>

      You can simulate the passive compass gait using:

      <pysrc>compass_gait/simulate.py</pysrc>

    </div>

    <p>Numerical integration of these equations reveals a stable limit cycle,
    plotted below.  Notice that when the left leg is in stance (top part of the
    plot), the trajectory quite resembles the familiar pendulum dynamics of the
    rimless wheel.  But this time, when the leg impacts, it takes a long arc
    around as the swing leg before returning to stance.  The impacts are also
    clearly visible as discontinuous changes (decreases) in velocity. The
    dependence of this limit cycle on the system parameters has been studied
    extensively in <elib>Goswami96a</elib>.</p>

    <figure> <img width="90%" data-src="figures/compass_gait_limit_cycle.svg"/>
    <figcaption>Passive limit cycle trajectory of the compass gait. ($m=5$kg,
    $m_h=10$kg, $a=b=0.5$m, $\phi=0.0525$rad. $\bx(0)=[0,0,0.4,-2.0]^T$).
    Drawn is the phase portait of the left leg angle, which is recovered from
    $\theta_{st}$ and $\theta_{sw}$ in the simulation with some simple
    book-keeping.</figcaption> </figure>

    <p>The basin of attraction of the stable limit cycle is a narrow band of
    states surrounding the steady state trajectory.  Although the simplicity of
    this model makes it analytically attractive, this lack of stability makes it
    difficult to implement on a physical device.</p>

  </section>

  <section data-type="sect1"><h1>The Kneed Walker</h1>

    <p>To achieve a more anthropomorphic gait, as well as to acquire better foot
    clearance and ability to walk on rough terrain, we want to model a walker
    that includes knee<elib>Hsu07</elib>. For this, we model each leg as two
    links with a point mass each.</p>

    <figure>
    <img width="60%" data-src="figures/kneedwalker.svg"/>
    <figcaption>The Kneed Walker</figcaption>
    </figure>

    <p>At the beginning of each step, the system is modeled as a three-link
    pendulum, like the ballistic walker<elib>McMahon80+Mochon80+Spong03</elib>.
    The stance leg is the one in front, and it is the first link of a pendulum,
    with two point masses. The swing leg has two links, with the joint between
    them unconstrained until knee-strike. Given appropriate mass distributions
    and initial conditions, the swing leg bends the knee and swings forward.
    When the swing leg straightens out (the lower and upper length are aligned),
    knee-strike occurs. The knee-strike is modeled as a discrete inelastic
    collision, conserving angular momentum and changing velocities
    instantaneously.</p>

    <p>After this collision, the knee is locked and we switch to the compass
    gait model with a different mass distribution. In other words, the system
    becomes a two-link pendulum. Again, the heel-strike is modeled as an
    inelastic collision. After the collision, the legs switch instantaneously.
    After heel-strike then, we switch back to the ballistic walker's three-link
    pendulum dynamics. This describes a full step cycle of the kneed walker,
    which is shown above.</p>

    <figure>
    <img width="100%" data-src="figures/kwcycle1.svg"/>
    <figcaption>Limit cycle trajectory for kneed walker</figcaption>
    </figure>

    <p>By switching between the dynamics of the continuous three-link and
    two-link pendulums with the two discrete collision events, we characterize a
    full point-feed kneed walker walking cycle. After finding appropriate
    parameters for masses and link lengths, a stable gait is found. As with the
    compass gait's limit cycle, there is a swing phase (top) and a stance phase
    (bottom). In addition to the two heel-strikes, there are two more
    instantaneous velocity changes from the knee-strikes as marked in the
    figure. This limit cycle is traversed clockwise.</p>

    <figure>
      <img width="90%" data-src="figures/kwLimitCycle.svg"/>
    <figcaption>Limit cycle (phase portrait) of the kneed walker</figcaption>
    </figure>

  </section>

</section> <!-- ***************  end of walking and running **************   -->

<section data-type="chapter" class="chapter"
id="humanoids"><h1>Highly-articulated Legged Robots</h1>

  <p>The passive dynamic walkers and hopping robots described in the last
  chapter capture the fundamental dynamics of legged locomotion -- dynamics
  which are fundamentally nonlinear and punctuated by impulsive events due to
  making and breaking contact with the environment.  But if you start reading
  the literature on humanoid robots, or many-legged robots like quadrupeds, then
  you will find a quite different set of ideas taking center stage: ideas like
  the "zero-moment point" and footstep planning.  The goal of this chapter is to
  penetrate that world.</p>

  <p>I'd like to start the discussion with a model that might seem quite far
  from the world of legged robots, but I think it's a very useful way to think
  about the problem.</p>

  <section data-type="sect1"><h1>Center of Mass Dynamics</h1>

    <section data-type="sect2"><h1>A hovercraft model</h1>

      <figure><img width="70%" data-src="figures/hovercraft.svg"/></figure>
      <todo>flip the directions of the force vector in the image</todo>

      <p>Imagine you have a flying vehicle modeled as a single rigid body in a
      gravity field with some number of force "thrusters" attached.  We'll
      describe the configuration of the vehicle by its orientation, $\theta$ and
      the location of its center of mass $(x,z)$.  The vector from the center of
      mass to thruster $i$ is given by $r_i$, yielding the equations of motion:
      \begin{align*} m\ddot{x} =& \sum_i F_{i,x},\\ m\ddot{z} =& \sum_i F_{i,z}
      - mg,\\ I\ddot\theta =& \sum_i \left[ r_i \times F_i \right]_y,
      \end{align*} where I've used $F_{i,x}$ to represent the component of the
      force in the $x$ direction and have taken just the $y$-axis component of
      the cross product on the last line. </p>

      <p>Our goal is to move the hovercraft to a desired position/orientation
      and to keep it there.  If we take the inputs to be $F_i$, then the
      dynamics are affine (linear plus a constant term).  As such, we can
      stabilize a stabilizable fixed point using a change of coordinates plus
      LQR or even plan/track a desired trajectory using time-varying LQR.  If I
      were to add additional linear constraints, for instance constraining
      $F_{min} \le F_i \le F_{max}$, then I can still use linear
      model-predictive control (MPC) to plan and stabilize a desired motion. By
      most accounts, this is a relatively easy control problem. (Note that it
      would be considerably harder if my control input or input constraints
      depend on the orientation, $\theta$, but we don't need that here yet).</p>

      <p>Now imagine that you just a small number of thrusters (let's say two)
      each with severe input constraints.  To make things more interesting, let
      us say that you're allowed to move the thrusters, so $\dot{r}_i$ becomes
      an additional control input, but with some important limitations:  you
      have to turn the thrusters off when they are moving (e.g.
      $|F_i||\dot{r}_i| = 0$) and there is a limit to how quickly you can move
      the thrusters $|\dot{r}|_i \le v_{max}$.  This problem is now a lot more
      difficult, due especially to the fact that constraints of the form $u_1
      u_2 = 0$ are non-convex.</p> <center><img width="25%"
      data-src="figures/complementarityConstraint.svg"/></center> <p>I find this
      a very useful thought exercise; somehow our controller needs to make an
      effectively discrete decision to turn off a thruster and move it.  The
      framework of optimal control should support this - you are sacrificing a
      short-term loss of control authority for the long-term benefit of having
      the thruster positioned where you would like, but we haven't developed
      tools yet that deal well with this discrete plus continuous decision
      making.  We'll need to address that here.</p>

      <p>Unfortunately, although the problem is already difficult, we need to
      continue to add constraints.  Now let's say additionally, that the
      thrusters can only be turned on in certain locations, as cartooned
      here:</p> <center><img width="60%"
      data-src="figures/hovercraftCartesianConstraints.svg"/></center> <p>The
      union of these regions need not form a convex set.  Furthermore, these
      locations could be specified in the robot frame, but may also be specified
      in the world frame, which I'll call ${\bf p}_i$: $${\bf p}_i = {\bf r}_i -
      \begin{bmatrix} x \\ 0 \\ z \end{bmatrix}.$$  This problem still feels
      like it should be tractable, but it's definitely getting hard.</p>

    </section>

    <section data-type="sect2"><h1>Robots with (massless) legs</h1>

      <p>In my view, the hovercraft problem above is a central component of the
      walking problem.  If we consider a walking robot with massless legs, then
      the feet are exactly like movable thrusters.  As above, they are highly
      constrained - they can only produce force when they are touching the
      ground, and (typically) they can only produce forces in certain
      directions, for instance as described by a "friction cone" (you can push
      on the ground but not pull on the ground, and with Coulomb friction the
      forces tangential to the ground must be smaller than the forces normal to
      the ground, as described by a coefficient of friction, e.g.
      $|F_{\parallel}| < \mu |F_{\perp}|$).</p>

      <figure><img width="50%" data-src="figures/hovercraftLegs.svg"/></figure>

      <p>The constraints on where you can put your feet / thrusters will depend
      on the kinematics of your leg, and the speed at which you can move them
      will depend on the full dynamics of the leg -- these are difficult
      constraints to deal with.  But the actual dynamics of the rigid body are
      actually still affine, and still simple!</p>

    </section>

    <section data-type="sect2"><h1>Capturing the full robot dynamics</h1>

      <p>We don't actually need to have massless legs for this discussion to
      make sense. If we use the coordinates $x,z$ to describe the location of
      the center of mass (CoM) of the entire robot, and $m$ to represent the
      entire mass of the robot, then the first two equations remain unchanged.
      The center of mass is a configuration dependent point, and does not have
      an orientation, but one important generalization of the orientation
      dynamics is given by the centroidal momentum matrix, $A(\bq)$, where
      $A(\bq)\dot{\bq}$ captures the linear and angular momentum of the robot
      around the center of mass <elib>Orin13</elib>. Note that the center of
      mass dynamics are still affine -- even for a big complicated humanoid --
      but the centroidal momentum dynamics are nonlinear.</p>

    </section>

    <section data-type="sect2"><h1>Impact dynamics</h1>

      <p>In the previous chapter we devoted relatively a lot of attention to
      dynamics of impact, characterized for instance by a guard that resets
      dynamics in a hybrid dynamical system.  In those models we used impulsive
      ground-reaction forces (these forces are instantaneously infinite, but
      doing finite work) in order to explain the discontinuous change in
      velocity required to avoid penetration with the ground.  This story can be
      extended naturally to the dynamics of the center of mass.<p>

      <p>For an articulated robot, though, there are a number of possible
      strategies for the legs which can effect the dynamics of the center of
      mass.  For example, if the robot hits the ground with a stiff leg like the
      rimless wheel, then the angular momentum about the point of collision will
      be conserved, but any momentum going into the ground will be lost.
      However, if the robot has a spring leg and a massless toe like the SLIP
      model, then no energy need be lost.</p>

      <p>One strategy that many highly-articulated legged robots use is to keep
      their center of mass at a constant height, $$z = c \quad \Rightarrow \quad
      \dot{z} = \ddot{z} = 0,$$  and minimize their angular momentum about the
      center of mass (here $\ddot\theta=0$).  Using this strategy, if the
      swinging foot lands roughly below the center of mass, then even with a
      stiff leg there is no energy dissipated in the collision - all of the
      momentum is conserved.  This often (but does not always) justify ignoring
      the impacts in the center of mass dynamics of the system.</p>

    </section>

  </section> <!-- end COM dynamics -->

  <section data-type="sect1"><h1>The special case of flat terrain</h1>

    <p>While not the only important case, it is extremely common for our robots
    to be walking over flat, or nearly flat terrain.  In this situation, even if
    the robot is touching the ground in a number of places (e.g., two heels and
    two toes), the constraints on the center of mass dynamics can be summarized
    very efficiently.</p>

    <figure>
      <img width="80%" data-src="figures/flat_terrain.svg"/>
      <figcaption>External forces acting on a robot pushing on a flat ground</figcaption>
    </figure>

    <p>First, on flat terrain $F_{i,z}$ represents the force that is normal to
    the surface at contact point $i$.  If we assume that the robot can only push
    on the ground (and not pull), then this implies $$\forall i, F_{i,z} \ge 0
    \Rightarrow \sum_i F_{i,z} \ge 0 \Rightarrow \ddot{z} \ge -g.$$  In other
    words, if I cannot pull on the ground, then my center of mass cannot
    accelerate towards the ground faster than gravity.</p>

    <p>Furthermore, if we use a Coulomb model of friction on the ground, with
    friction coefficient $\mu$, then $$\forall i, |F_{i,x}| \le \mu F_{i,z}
    \Rightarrow \sum_i |F_{i,x}| \le \mu \sum_i F_z \Rightarrow |\ddot{x}| \le
    \mu (\ddot{z} + g).$$  For instance, if I keep my center of mass at a
    constant height, then $\ddot{z}=0$ and $|\ddot{x}| \le \mu g$; this is a
    nice reminder of just how important friction is if you want to be able to
    move around in the world.</p>

    <p>Even better, let us define the "center of pressure" (CoP) as the point on
    the ground where $$x_{cop} = \frac{\sum_i p_{i,x} F_{i,z}}{\sum_i
    F_{i,z}},$$ and since all $p_{i,z}$ are equal on flat terrain, $z_{cop}$ is
    just the height of the terrain.  It turns out that the center of pressure is
    a "zero-moment point" (ZMP) -- a property that we will demonstrate below --
    and moment-balance equation gives us a very important relationship between
    the location of the CoP and the dynamics of the CoM: \[ (m\ddot{z} + mg)
    (x_{cop} - x) = (z_{cop} - z) m\ddot{x} - I\ddot\theta. \]  If we use the
    strategy proposed above for ignoring collision dynamics, $\ddot{z} =
    \ddot{\theta} = 0$, then we have $z_{cop} - z$ is a constant height $h$, and
    the result is the famous "ZMP equations": \[ \ddot{x} = -\frac{g}{h}
    (x_{cop}-x). \]  So the location of the center of pressure completely
    determines the acceleration of the center of mass, and vice versa!  What's
    more, this relationship is affine -- a property that we can exploit in a
    number of ways.  </p>

    <p> As an example, we can easily relate constraints on the CoP to
    constraints on $\ddot{x}$. It is easy to verify from the definition that the
    CoP must live inside the <a
    href="http://en.wikipedia.org/wiki/Convex_hull">convex hull</a> of the
    ground contact points.  Therefore, if we use the $\ddot{z}=\ddot\theta=0$
    strategy, then this directly implies strict bounds on the possible
    accelerations of the center of mass given the locations of the ground
    contacts.</p>

    <section data-type="sect2"><h1>An aside: the zero-moment point
    derivation</h1>

      <p>The zero-moment point (ZMP) is discussed very frequently in the current
      literature on legged robots.  It also has an unfortunate tendency to be
      surrounded by some confusion; a number of people have defined the ZMP is
      slightly different ways (see e.g. <elib>Goswami99</elib> for a summary).
      Therefore, it makes sense to provide a simple derivation here.</p>

      <p>First let us recall that for rigid body systems I can always summarize
      the contributions from many external forces as a single <i>wrench</i>
      (force and torque) on the object -- this is simply because the $F_i$ terms
      enter our equations of motion linearly.  Specifically, given any point in
      space, $r$, in coordinates relative to $(x,z)$:</p> <figure><img
      width="70%" data-src="figures/zmp_derivation.svg"/></figure> <todo>update
      diagram to have r instead of p</todo>

      <p>I can re-write the equations of motion as \begin{align*} m\ddot{x} =&
      \sum_i F_{i,x} = F_{net,x},\\ m\ddot{z} =& \sum_i F_{i,z} - mg = F_{net,z}
      - mg,\\ I\ddot\theta =& \sum_i \left[ r_i \times F_i \right]_y = ({\bf r}
      \times {\bf F}_{net})_y + \tau_{net}, \end{align*} where ${\bf F}_{net} =
      \sum_i {\bf F}_i$ and the value of $\tau_{net}$ depends on the location
      ${\bf r}$.  For some choices of ${\bf r}$, we can make $\tau_{net}=0$.
      Examining \[ ({\bf r} \times {\bf F}_{net})_y = r_z F_{net,x} - r_x
      F_{net,z} = \left[ r_i \times F_i \right]_y, \] we can see that when
      $F_{net,z} > 0$ there is an entire line of solutions, $r_x = a r_z + b$,
      including one that will intercept the terrain.  For walking robots, it is
      this point on the ground from which the external wrench can be described
      by a single force vector (and no moment) that is the famous "zero-moment
      point" (ZMP).     Substituting the back in to replace $F_{net}$, we can
      see that \[ r_x = \frac{r_z m \ddot{x} - I \ddot\theta}{m\ddot{z} + mg}.
      \] If we assume that $\ddot{z}=\ddot{\theta}=0$ and replace the relative
      coordinates with the global coordinates ${\bf r} = {\bf p} - [x,0,z]^T$,
      then we arrive at precisely the equation presented above.</p>

      <p>Furthermore, since \[\left[ r_i \times F_i \right]_y = \sum_i \left(
      r_{i,z} F_{i,x} - r_{i,x} F_{i,z} \right), \] and for <i>flat terrain</i>
      we have \[ r_z F_{net,x} = \sum_i r_{i,z} F_{i,x}, \] then we can see that
      this ZMP is exactly the CoP: \[ r_x = \frac{\sum_i r_{i,x} F_{i,z}}{
      F_{net,z} }. \] </p>

      <p>In three dimensions, we solve for the point on the ground where
      $\tau_{net,y} = \tau_{net,x} = 0$, but allow $\tau_{net,z} \ne 0$ to
      extract the analogous equations in the $y$-axis:  \[ r_y = \frac{r_z m
      \ddot{y} - I \ddot\theta}{m\ddot{z} + mg}. \] </p>

    </section>
  </section>

  <section data-type="sect1"><h1>ZMP planning</h1>

    <figure><img width="80%" data-src="figures/zmp_planning.svg"/></figure>

    <section data-type="sect2"><h1>From a CoM plan to a whole-body
    plan</h1></section>

  </section>


  <section data-type="sect1"><h1>Whole-Body Control</h1>

    <p>Coming soon.  For a description of our approach with Atlas, see
    <elib>Kuindersma13+Kuindersma14</elib>.</p>

  </section>

  <section data-type="sect1"><h1>Footstep planning and push recovery</h1>

    <p>Coming soon.  For a description of our approach with Atlas, see
    <elib>Deits14a+Kuindersma14</elib>.  For nice geometric insights on push
    recovery, see <elib>Koolen12</elib>.</p>

  </section>

  <section data-type="sect1"><h1>Beyond ZMP planning</h1>

    <p>Coming soon.  For a description of our approach with Atlas, see
    <elib>Dai14+Kuindersma14</elib>.</p>

  </section>

</section> <!-- ** end highly-articulated legged robots ** -->

<section data-type="chapter" class="chapter" id="manip"><h1>Manipulation</h1>

<todo>Sense, plan, act.  Grasp planning (grasp metrics).  Underactuated hands.
  Feedback.  Not: perception (only mention its importance).</todo>

</section>

<!-- ***************  end of Manipulation **************   -->

<!--
<section data-type="chapter" class="chapter" id="uavs"><h1>Unmanned Aerial
Vehicles</h1>

</section> <!-- ***************  end of uavs **************   -->

<!--
<section data-type="chapter" class="chapter" id="automotive"><h1>Autonomous
Driving</h1>

</section> <!-- ***************  end of vehicles **************   -->

<!--
<section data-type="chapter" class="chapter" id="fluids"><h1>Model Systems with
  Fluid
  Dynamics</h1>


</section> <!-- ***************  end of fluid dynamics **************   -->

<!--
<section data-type="chapter" class="chapter" id="soft"><h1>Soft Robots</h1>


</section> <!-- ***************  end of soft robots **************   -->

<section data-type="chapter" class="chapter" id="stochastic"><h1>Model Systems
with Stochasticity</h1>

  <p>My goals for this chapter are to build intuition for the beautiful and rich
  behavior of nonlinear dynamical system that are subjected to random
  (noise/disturbance) inputs.  So far we have focused primarily on systems
  described by \[ \dot{\bx}(t) = f(\bx(t),\bu(t)) \quad \text{or} \quad \bx[n+1]
  = f(\bx[n],\bu[n]). \] In this chapter, I would like to broaden the scope to
  think about \[ \dot{\bx}(t) = f(\bx(t),\bu(t),\bw(t)) \quad \text{or} \quad
  \bx[n+1] = f(\bx[n],\bu[n],\bw[n]), \] where this additional input $\bw$ is
  the (vector) output of some random process.  In other words, we can begin
  thinking about stochastic systems by simply understanding the dynamics of our
  existing ODEs subjected to an additional random input. </p>

  <p> This form is extremely general as written.  $\bw(t)$ can represent
  time-varying random disturbances (e.g. gusts of wind), or even constant model
  errors/uncertainty.  One thing that we are not adding, yet, is measurement
  uncertainty.  There is a great deal of work on observability and state
  estimation that study the question of how you can infer the true state of the
  system given noise sensor readings.  For this chapter we are assuming perfect
  measurements of the full state, and are focused instead on the way that
  "process noise" shapes the long-term dynamics of the system. </p>

  <p> I will also stick primarily to discrete time dynamics for this chapter,
  simply because it is easier to think about the output of a discrete-time
  random process, $\bw[n]$, than a $\bw(t)$. But you should know that all of the
  ideas work in continuous time, too.  Also, most of our examples will take the
  form of <i>additive noise</i>: \[ \bx[n+1] = f(\bx[n],\bu[n]) + \bw[n], \]
  which is a particular useful and common specialization of our general form.
  And this form doesn't give up much -- the disturbance on step $k$ can pass
  through the nonlinear function $f$ on step $k+1$ giving rich results -- but is
  often much easier to work with. </p>

  <section data-type="sect1"><h1>The Master Equation</h1>

    <p>Let's start by looking at some simple examples.</p>

    <div data-type="example"><h1>A Bistable System + Noise</h1>

      <p>Let's consider (a time-reversed version of) one of my favorite
      one-dimensional systems: \[ \dot{x} = x - x^3. \] <todo>add plot</todo>
      This deterministic system has stable fixed points at $x^* = \{-1,1\}$ and
      an unstable fixed point at $x^* = 0$. </p> <p> A reasonable discrete-time
      approximation of these dynamics, now with additive noise, is \[ x[n+1] =
      x[n] + h (x[n] - x[n]^3 + w[n]). \] When $w[n]=0$, this system has the
      same fixed points and stability properties as the continuous time system.
      But let's examine the system when $w[n]$ is instead the result of a
      <i>zero-mean Gaussian white noise process</i>, defined by: \begin{gather*}
      \forall n, E\left[ w[n] \right] = 0,\\ E\left[ w[i]w[j] \right] =
      \begin{cases} \sigma^2, & \text{ if } i=j,\\ 0, & \text{ otherwise.}
      \end{cases} \end{gather*} Here $\sigma$ is the standard deviation of the
      Gaussian. </p> <p>When you simulate this system for small values of
      $\sigma$, you will see trajectories move roughly towards one of the two
      fixed points (for the deterministic system), but every step is modified by
      the noise.  In fact, even if the trajectory were to arrive exactly at what
      was once a fixed point, it is almost surely going to move again on the
      very next step.  In fact, if we plot many runs of the simulation from
      different initial conditions all on the same plot, we will see something
      like the figure below.</p> <figure> <img width="80%"
      data-src="figures/cubic_bistable_particles_time.svg"/>
      <figcaption>Simulation of the bistable system with noise ($\sigma = 0.01$)
      from many initial conditions.</figcaption> </figure>

      <p>During any individual simulation, the state jumps around randomly for
      all time, and could even transition from the vicinity of one fixed point
      to the other fixed point.  Another way to visualize this output is to
      animate a histogram of the particles over time.</p>

      <figure> <img width="80%"
      data-src="figures/cubic_bistable_histogram.svg"/> <figcaption>Histogram of
      the states of the bistable system with noise ($\sigma = 0.01$) after
      simulating from random initial conditions until $t=30$. <br/><a
      href="figures/cubic_bistable_histogram.swf">Click here to see the
      animation</a></figcaption> </figure>

      <p>You can run this demo for yourself:</p>
      <pysrc>simple/stochastic_system_particle_demo.py</pysrc>

    </div>

    <p>Let's take a moment to appreciate the implications of what we've just
    observed. Every time that we've analyzed a system to date, we've asked
    questions like "given x[0], what is the long-term behavior of the system,
    $\lim_{n\rightarrow\infty} x[n]$?", but now $x[n]$ is a <i>random
    variable</i>. The trajectories of this system do not converge, and the
    system does not exhibit any form of stability that we've introduced so far.
    </p>

    <p>All is not lost.  If you watch the animation closely, you might notice
    the <i>distribution</i> of this random variable is actually very well
    behaved.  This is the key idea for this chapter.</p>

    <p>Let us use $p_n(x)$ to denote the <a
    href="https://en.wikipedia.org/wiki/Probability_density_function">
    probability density function</a> over the random variable $x$ at time $n$.
    Note that this density function must satisfy \[ \int_{-\infty}^\infty p_n(x)
    dx = 1.\] It is actually possible to write the <i>dynamics of the
    probability density</i> with the simple relation \[ p_{n+1}(x) =
    \int_{-\infty}^\infty p(x|x') p_n(x') dx', \] where $p(x|x')$ encodes the
    stochastic dynamics as a conditional distribution of the next state (here
    $x$) as a function of the current state (here $x'$).  Dynamical systems that
    can be encoded in this way are known as <i>continuous-state Markov
    Processes</i>, and the governing equation above is often referred to as the
    "<a href="https://en.wikipedia.org/wiki/Master_equation">master
    equation</a>" for the stochastic process.  In fact this update is even
    linear(!) ; a fact that can enable closed-form solutions to some impressive
    long-term statistics, like mean time to failure or first passage
    times<elib>Byl08a</elib>. Unfortunately, it is often difficult to perform
    the integral in closed form, so we must often resort to discretation or
    numerical analysis. </p>

    <p>The slightly more general form of the master equation, which works for
    multivariable distributions with state-domain ${\cal X}$, and systems with
    control inputs $\bu$, is \[ p_{n+1}(\bx) = \int_{\cal X} p(\bx|\bx',\bu)
    p_n(\bx') d\bx'. \] This is a (continuous-state) Markov Decision
    Process.</p>

  </section>

  <section data-type="sect1"><h1>Stationary Distributions</h1>

    <p>In the example above, the histogram is our numerical approximation of the
    probability density. If you simulate it a few times, you will probably
    believe that, although the individual trajectories of the system <i>do
    not</i> converge, the probability distribution actually <i>does</i> converge
    to what's known as a <i>stationary distribution</i> -- a fixed point of the
    master equation.  Instead of thinking about the dynamics of the
    trajectories, we need to start thinking about the dynamics of the
    distribution.</p>

    <div data-type="example"><h1>The Linear Gaussian System</h1>

      <p>Let's consider the one-dimensional linear system with additive noise:
      \[ x[n+1] = a x[n] + w[n]. \] When $w[n]=0$, the system is stable (to the
      origin) for $-1 < a < 1$.  Let's make sure that we can understand the
      dynamics of the master equation for the case when $w[n]$ is Gaussian white
      noise with standard deviation $\sigma$. </p>

      <p>First, recall that the probability density function of a Gaussian with
      mean $\mu$ is given by \[ p(w) = \frac{1}{\sqrt{2 \pi \sigma^2}}
      e^{-\frac{(w-\mu)^2}{2\sigma^2}}. \] When conditioned on $x[n]$, the
      distribution given by the dynamics subjected to mean-zero Gaussian white
      noise is simply another Gaussian, with the mean given by $ax[n]$: \[
      p(x[n+1]|x[n]) = \frac{1}{\sqrt{2 \pi \sigma^2}}
      e^{-\frac{(x[n+1]-ax[n])^2}{2\sigma^2}}, \] yielding the master equation
      \[ p_{n+1}(x) = \frac{1}{\sqrt{2 \pi \sigma^2}} \int_{-\infty}^\infty
      e^{-\frac{(x-ax')^2}{2\sigma^2}} p_n(x') dx'. \] </p>

      <p>Now here's the magic.  Let's push a distribution, $p_n(x)$, which is
      zero-mean, with standard deviation $\sigma_n$, through the master
      equation: \begin{align*} p_{n+1}(x) =& \frac{1}{\sqrt{2 \pi
      \sigma^2}}\frac{1}{\sqrt{2 \pi \sigma_n^2}} \int_{-\infty}^\infty
      e^{-\frac{(x-ax')^2}{2\sigma^2}} e^{-\frac{(x')^2}{2\sigma_n^2}} dx',\\ =&
      \frac{1}{\sqrt{2 \pi (\sigma^2+a^2 \sigma_n^2)}} e^{-\frac{x^2}{2(\sigma^2
      + a^2 \sigma_n^2)}}. \end{align*} The result is another mean-zero Gaussian
      with $\sigma_{n+1}^2 = \sigma^2 + a^2 \sigma_n^2$.  This result
      generalizes to the multi-variate case, too, and might be familiar to you
      e.g. from the process update of the <a
      href="https://en.wikipedia.org/wiki/Kalman_filter">Kalman filter</a>. </p>

      <p>Taking it a step further, we can see that a stationary distribution for
      this system is given by a mean-zero Gaussian with \[ \sigma_*^2 =
      \frac{\sigma^2}{1-a^2}. \] Note that this distribution is well defined
      when $-1 < a < 1$ (only when the system is stable).</p>

    </div>

    <p>The stationary distribution of the linear Gaussian system reveals the
    fundamental and general balance between two terms in the governing equations
    of any stochastic dynamical system: the stability of the deterministic
    system is bringing trajectories together (smaller $a$ means faster
    convergence of the deterministic system and results in a more narrow
    distribution), but the noise in the system is forcing trajectories apart
    (larger $\sigma$ means larger noise and results in a wider distribution).
    </p>

    <p>Given how rich the dynamics can be for deterministic nonlinear systems,
    you can probably imagine that the possible long-term dynamics of the
    probability are also extremely rich.  If we simply flip the signs in the
    dynamics we examined above, we'll get our next example:</p>

    <div data-type="example"><h1>The Cubic Example + Noise</h1>

      <p>Now let's consider the discrete-time approximation of \[ \dot{x} = -x +
      x^3, \] again with additive noise: \[ x[n+1] = x[n] + h (-x[n] + x[n]^3 +
      w[n]). \] With $w[n]=0$, the system has only a single stable fixed point
      (at the origin), whose deterministic region of attraction is given by $x
      \in (-1,1)$.  If we again simulate the system from a set of random initial
      conditions and plot the histogram, we will see something like the figure
      below. </p>

      <figure> <img width="80%" data-src="figures/cubic_histogram.svg"/>
      <figcaption>Histogram of the states of the bistable system with noise
      ($\sigma = 0.01$) after simulating from random initial conditions until
      $t=30$. <br/><a href="figures/cubic_histogram.swf">Click here to see the
      animation</a></figcaption> </figure>

      <p>Be sure to watch the animation.  Or better yet, run the simulation
        for yourself by changing the sign of the derivative in the example
        and re-running:</p>

      <pysrc>simple/stochastic_system_particle_demo.py</pysrc>

      <p>Now try increasing the noise (e.g. pre-multiply the noise input,
        $w$, in the dynamics by a scalar like 2).</p>

      <p style="text-align:center"><a
      href="figures/cubic_histogram_more_noise.swf">Click here to see the
      animation</a></p>

      <p>What is the stationary distribution for this system?  In fact, there
      isn't one.  Although we initially collect probability density around the
      stable fixed point, you should notice a slow leak -- on every step there
      is some probability of transitioning past unstable fixed points and
      getting driven by the unstable dynamics off towards infinity.  If we run
      the simulation long enough, there won't be any probability density left at
      $x=0$.</p>
    </div>

    <div data-type="example"><h1>The Stochastic Van der Pol Oscillator</h1>

      <p>One more example; this is a fun one.  Let's think about one of the
      simplest examples that we had for a system that demonstrates limit cycle
      stability -- the Van der Pol oscillator -- but now we'll add Gaussian
      white noise, $$\ddot{q} + (q^2 - 1)\dot{q} + q = w(t),$$ Here's the
      question: if we start with a small set of initial conditions centered
      around one point on the limit cycle, then what is the long-term behavior
      of this distribution? </p>

      <figure> <img width="80%" data-src="figures/vanderpol_particles.svg"/>
      <figcaption>Randomly sampled initial conditions pictured with the stable
      limit cycle of the Van der Pol oscillator.</figcaption> </figure>

      <p>Since the long-term behavior of the deterministic system is periodic,
      it would be very logical to think that the state distribution for this
      stochastic system would fall into a stable periodic solution, too.  But
      think about it a little more, and then watch the animation (or run the
      simulation yourself).</p>

      <pysrc>van_der_pol/particles.py</pysrc>

      <p style="text-align:center"><a
      href="figures/vanderpol_particles.swf">Click here to see the animation
      (first 20 seconds)</a></p>

      <p style="text-align:center"><a
      href="figures/vanderpol_particles_mixed.svg">Click here to see the
      particles after 2000 seconds of simulation</a></p>

       <p>The explanation is simple: the periodic solution of the system is only
       <i>orbitally stable</i>; there is no stability along the limit cycle.  So
       any disturbances that push the particles along the limit cycle will go
       unchecked.  Eventually, the distribution will "mix" along the entire
       cycle. Perhaps surprisingly, this system that has a limit cycle stability
       when $w=0$ eventually reaches a stationary distribution (fixed point) in
       the master equation.</p>

    </div>

  </section>

  <section data-type="sect1"><h1>Extended Example: The Rimless Wheel on Rough
  Terrain</h1>

    <p>Coming soon.  Read the paper <elib>Byl08f</elib> and/or watch the <a
        href="https://youtu.be/TaaJgvjYBQc">video</a>.</p>

  </section>

  <section data-type="sect1"><h1>Noise models for real robots/systems.</h1>

    <p>Sensor models.  Beam model from probabilistic robotics.  RGB-D
    dropouts.</p>

    <p>Perception subsystem.  Output of a
    perception system is not Gaussian noise, it's missed
    detections/drop-outs...</p>

    <p>Distributions over tasks/environments.</p>

  </section>

  <section data-type="sect1"><h1>Analysis (a preview)</h1>

    <p>Coming soon.  Uncertainty quantification (e.g., linear guassian
    approximation; discretize then closed form solutions using the transition
    matrix....).  Monte-Carlo. Particle sim/filter. Rare event simulation </p>

    <p>L2-gain with dissipation inequalities.  Finite-time verification with
    sums of squares.</p>

  </section>


  <section data-type="sect1"><h1>Control Design (a preview)</h1>

  </section>

</section> <!-- ***************  end of stochasticity **************   -->


</div> <!-- end of Part: model systems -->


<div class="part" title="Nonlinear Planning and Control">

<section data-type="chapter" class="chapter" id="dp"><h1>Dynamic Programming</h1>

  <p>In chapter 2, we spent some time thinking about the phase portrait of the
  simple pendulum, and concluded with a challenge: can we design a nonlinear
  controller to <em>reshape</em> the phase portrait, with a very modest amount
  of actuation, so that the upright fixed point becomes globally stable?  With
  unbounded torque, feedback linearization solutions (e.g., invert gravity) can
  work well, but can also require an unnecessarily large amount of control
  effort. The energy-based swing-up control solutions presented for the acrobot
  and cart-pole systems  are considerably more appealing, but required some
  cleverness and might not scale to more complicated systems.  Here we
  investigate another approach to the problem, using computational optimal
  control to synthesize a feedback controller directly.</p>

  <section data-type="sect1"><h1>Formulating control design as an optimization</h1>

    <p>In this chapter, we will introduce optimal control - a control design
    process using optimization.  This approach is powerful for a number of
    reasons. First and foremost, it is very general - allowing us to   specify
    the goal of control equally well for fully- or under-actuated,   linear or
    nonlinear, deterministic or stochastic, and continuous or   discrete
    systems. Second, it permits concise descriptions of potentially very complex
    desired behaviours, specifying the goal of control as an scalar objective
    (plus a list of constraints).  Finally, and most importantly, optimal
    control is very amenable to numerical solutions. <elib>Bertsekas00a</elib>
    is a fantastic reference on this material.</p>

    <p>The fundamental idea in optimal control is to formulate the goal of
    control as the <em>long-term</em> optimization of a scalar cost function.
    Let's introduce the basic concepts by considering a system that is even
    simpler than the simple pendulum.</p>

    <div data-type="example"><h1>Optimal Control Formulations for the Double Integrator</h1>

      <p>Consider the double integrator system $$\ddot{q} = u, \quad |u| \le
      1.$$ If you would like a mechanical analog of the system (I always do),
      then you can think about this as a unit mass brick moving along the x-axis
      on a frictionless surface, with a control input which provides a
      horizontal force, $u$. The task is to design a control system, $u =
      \pi(\bx,t)$, $\bx=[q,\dot{q}]^T$ to regulate this brick to $\bx =
      [0,0]^T$.</p>

      <figure>
      <img width="70%" data-src="figures/double_integrator_brick.svg"/>
      <figcaption>The double integrator as a unit-mass brick on a frictionless
      surface</figcaption>
      </figure>

      <p>In order to formulate this control design problem using optimal
      control, we must define a scalar objective which scores the long-term
      performance of running each candidate control policy, $\pi(\bx,t)$, from
      each initial condition, $(\bx_0,t_0)$, and a list of constraints that must
      be satisfied. For the task of driving the double integrator to the origin,
      one could imagine a number of optimal control formulations which would
      accomplish the task, e.g.: <ul> <li> Minimum time:  $\min_\pi t_f,$
      subject to $\bx(t_0) = \bx_0,$ $\bx(t_f) = {\bf 0}.$ </li> <li> Quadratic
      cost:  $\min_\pi \int_{t_0}^{\infty} \left[ \bx^T(t) {\bf Q} \bx(t)
      \right] dt,$ ${\bf Q}\succ0$.</li> </ul> where the first is a constrained
      optimization formulation which optimizes time, and the second accrues a
      penalty at every instance according to the distance that the state is away
      from the origin (in a metric space defined by the matrix ${\bf Q}$), and
      therefore encourages trajectories that go more directly towards the goal,
      possibly at the expense of requiring a longer time to reach the goal (in
      fact it will result in an exponential approach to the goal, where as the
      minimum-time formulation will arrive at the goal in finite time). Note
      that both optimization problems only have well-defined solutions if it is
      possible for the system to actually reach the origin, otherwise the
      minimum-time problem cannot satisfy the terminal constraint, and the
      integral in the quadratic cost would not converge to a finite value as
      time approaches infinity (fortunately the double integrator system is
      controllable, and therefore can be driven to the goal in finite time).</p>

      <p> Note that the input limits, $|u|\le1$ are also required to make this
      problem well-posed; otherwise both optimizations would result in the
      optimal policy using infinite control input to approach the goal
      infinitely fast. Besides input limits, another common approach to limiting
      the control effort is to add an additional quadratic cost on the input (or
      "effort"), e.g. $\int \left[ \bu^T(t) {\bf R} \bu(t) \right] dt,$   ${\bf
      R}\succ0$. This could be added to either formulation above.  We will
      examine many of these formulations in some details in the examples worked
      out at the end of this chapter. </p>

    </div> <!-- end example -->

    <p>Optimal control has a long history in robotics.  For instance, there has
    been a great deal of work on the minimum-time problem for pick-and-place
    robotic manipulators, and the linear quadratic regulator (LQR) and linear
    quadratic regulator with Gaussian noise (LQG) have become essential tools
    for any practicing controls engineer.  With increasingly powerful computers
    and algorithms, the popularity of numerical optimal control has grown at an
    incredible pace over the last few years.</p>

    <div data-type="example"><h1>The minimum time problem for the double
    integrator</h1>

      <p>For more intuition, let's do an informal derivation of the solution to
      the minimum time problem for the double integrator with input constraints:
      \begin{align*}
        \minimize_{\pi} \quad & t_f\\
        \subjto \quad & \bx(t_0) = \bx_0, \\
        & \bx(t_f) = {\bf 0}, \\
        & \ddot{q}(t) = u(t), \\
        & |u| \le 1.
      \end{align*}
      What behavior would you expect an optimal controller exhibit?</p>

      <p> Your intuition might tell you that the best thing that the brick can
      do, to reach the goal in minimum time with limited control input, is to
      accelerate maximally towards the goal until reaching a critical point,
      then hitting the brakes in order to come to a stop exactly at the goal.
      This would be called a <em>bang-bang</em> control policy; these are often
      optimal for systems with bounded input, and it is in fact optimal for the
      double integrator, although we will not prove it until we have developed
      more tools.  <!-- leave the proof to the pontryagin notes --></p>

      <p> Let's work out the details of this bang-bang policy.  First, we can
      figure out the states from which, when the brakes are fully applied, the
      system comes to rest precisely at the origin.  Let's start with the case
      where $q(0) < 0$, and $\dot{q}(0)>0$, and "hitting the brakes" implies
      that $u=-1$ . Integrating the equations, we have \begin{gather*}
      \ddot{q}(t) = u = -1 \\\dot{q}(t) = \dot{q}(0) - t \\ q(t) = q(0) +
      \dot{q}(0) t - \frac{1}{2} t^2.  \end{gather*} Substituting $t =
      \dot{q}(0) - \dot{q}$ into the solution give $\dot{q}$ reveals that the
      system orbits are parabolic arcs: \[ q = -\frac{1}{2} \dot{q}^2 + c_{-},
      \] with $c_{-} = q(0) + \frac{1}{2}\dot{q}^2(0)$.</p>

      <figure>
        <img width="80%" data-src="figures/double_integrator_orbits.svg"/>
        <figcaption>Two solutions for the system with $u=-1$</figcaption>
      </figure>

      <!-- t = qdot - qdot0,
      q = q0 + qdot0(qdot-qdot0) + 1/2(qdot-qdot0)^2
        = q0 + qdot0*qdot - qdot0^2 + 1/2qdot^2 - qdot*qdot0 + 1/2qdot0^2
        = 1/2qdot^2 + (q0 - 1/2 qdot0^2)
      -->

      <p>Similarly, the solutions for $u=1$ are $q = \frac{1}{2} \dot{q}^2 +
      c_{+}$, with $c_{+}=q(0)-\frac{1}{2}\dot{q}^2(0)$.</p>

      <p> Perhaps the most important of these orbits are the ones that pass
      directly through the origin (e.g., $c_{-}=0$). Following our initial
      logic, if the system is going slower than this $\dot{q}$ for any $q$, then
      the optimal thing to do is to slam on the accelerator
      ($u=-\text{sgn}(q)$).  If it's going faster than the $\dot{q}$ that we've
      solved for, then still the best thing to do is to brake; but inevitably
      the system will overshoot the origin and have to come back.  We can
      summarize this policy with: \[ u = \begin{cases}  +1 & \text{if } (\dot{q}
      < 0 \text{ and } q \le \frac{1}{2} \dot{q}^2) \text{ or } (\dot{q}\ge 0
      \text{ and } q < -\frac{1}{2} \dot{q}^2) \\ 0 & \text{if } q=0 \text{ and
      } \dot{q}=0 \\ -1 & \text{otherwise} \end{cases} \]</p> <!--This policy is
      cartooned in Figure~\ref{fig:mintime_double_int}.  %Trajectories of the
      system %executing this policy are also included - the fundamental
      %characteristic is that the system is accelerated as quickly as %possible
      toward the switching surface, then rides the switching %surface in to the
      origin. -->

      <figure>
      <img width="80%" data-src="figures/double_integrator_mintime_policy.svg"/>
      <figcaption>Candidate optimal (bang-bang) policy for the minimum-time
        double integrator problem.</figcaption>
      </figure>

      <p>and illustrate some of the optimal solution trajectories:</p>

      <figure>
      <img width="80%" data-src="figures/double_integrator_mintime_orbits.svg"/>
      <figcaption>Solution trajectories of system using the optimal policy</figcaption>
      </figure>

      <p>And for completeness, we can compute the optimal time to the goal by
      solving for the amount of time required to reach the switching surface
      plus the amount of time spent moving along the switching surface to the
      goal.  With a little algebra, you will find that the time to goal,
      $T(\bx)$, is given by \[ T(\bx) = \begin{cases}
      2\sqrt{\frac{1}{2}\dot{q}^2-q} - \dot{q} & \text{for } u=+1 \text{
      regime}, \\ 0 & \text{for } u=0, \\ \dot{q} +
      2\sqrt{\frac{1}{2}\dot{q}^2+q} & \text{for } u=-1, \end{cases} \]<!-- call
      t_m the time to the surface, then the time on switching surface =
      |qdot(t_m)|
      for u=1
      q0,qdot0 => qm,qdotm with qm = -1/2 qdotm^2
      -1/2 qdotm^2 = 1/2 qdotm^2 + c+
      qdotm^2 = - c+  , qdotm = sqrt(-c+)
      T = (qdotm-qdot0)+qdotm = 2*sqrt(-c+) - qdot0
      for u=-1, qdotm^2 = c- , qdotm = -sqrt(c-)
      T = (qdot0-qdotm)-qdotm = qdot0 + 2*sqrt(c-)
      -->
      plotted here:</p>

      <figure>
      <img width="80%" data-src="figures/double_integrator_mintime_cost_to_go.svg"/>
      <figcaption>Time to the origin using the bang-bang policy</figcaption>
      </figure>

      <p> Notice that the function is continuous (though not smooth), even
      though the policy is discontinuous.</p>

    </div> <!-- end of example -->

    <section data-type="sect2"><h1>Additive cost</h1>

      <p> As we begin to develop theoretical and algorithmic tools for optimal
      control, we will see that some formulations are much easier to deal with
      than others. One important example is the dramatic simplification that can
      come from formulating objective functions using <em>additive cost</em>,
      because they often yield recursive solutions.  In the additive cost
      formulation, the long-term "score" for a trajectory can be written as
      $$\int_0^T g(x(t),u(t)) dt,$$ where $g()$ is the instantaneous cost, and
      $T$ can be either a finite real number or $\infty$.  We will call a
      problem specification with a finite $T$ a "finite-horizon" problem, and
      $T=\infty$ an "infinite-horizon" problem.  Problems and solutions for
      infinite-horizon problems tend to be more elegant, but care is required to
      make sure that the integral converges for the optimal controller
      (typically by having an achievable goal that allows the robot to accrue
      zero-cost).</p>

      <p> The quadratic cost function suggested in the double integrator example
      above is clearly written as an additive cost.  At first glance, our
      minimum-time problem formulation does appear to be of this form, but we
      actually can write it as an additive cost problem using an infinite
      horizon and the instantaneous cost  $$g(x,u) = \begin{cases} 0 & \text{if
      } x=0, \\ 1 & \text{otherwise.} \end{cases}$$</p>

      <p> We will examine a number of approaches to solving optimal control
      problems throughout the next few chapters.  For the remainder of this
      chapter, we will focus on additive-cost problems and their solution via
      <em>dynamic programming</em>.</p>

    </section> <!-- end of additive cost -->

  </section> <!-- control design as an optimization -->

  <section data-type="sect1"><h1>Optimal control as graph search</h1>

    <p> For systems with continuous states and continuous actions, dynamic
    programming is a set of theoretical ideas surrounding additive cost optimal
    control problems. For systems with a finite, discrete set of states and a
    finite, discrete set of actions, dynamic programming also represents a set
    of very efficient numerical <em>algorithm</em> which can compute optimal
    feedback controllers. Many of you will have learned it before as a tool for
    graph search. </p>

    <p>Imagine you have a directed graph with states (or nodes) $\{s_1,s_2,...\}
    \in S$ and "actions" associated with edges labeled as $\{a_1,a_2,...\} \in
    A$, as in the following trivial example:</p>

    <figure><img width="70%" data-src="figures/graph_search.svg"/><figcaption>A
    simple directed graph.</figcaption></figure>

    <p>Let us also assume that each edge has an associate weight or cost, using
    $g(s,a)$ to denote the cost of being in state $s$ and taking action $a$.
    Furthermore we will denote the transition "dynamics" using \[ s[n+1] =
    f(s[n],a[n]). \] For instance, in the graph above, $f(s_1,a_1) = s_2$.</p>

    <p>There are many algorithms for finding (or approximating) the optimal path
    from a start to a goal on directed graphs.  In dynamic programming, the key
    insight is that we can find the shortest path from every node by solving
    recursively for the optimal <em>cost-to-go</em> from every node to the goal.
    One such algorithm starts by initializing an estimate $\hat{J}^*=0$ for all
    $s_i$, then proceeds with an iterative algorithm which sets \begin{equation}
    \hat{J}^*(s_i) \Leftarrow \min_{a \in A} \left[ g(s_i,a) +
    \hat{J}^*\left({f(s_i,a)}\right) \right]. \label{eq:value_update}
    \end{equation} In software, $\hat{J}^*$ can be represented as a vector with
    dimension equal to the number of discrete states.  This algorithm,
    appropriately known as <em>value iteration</em>, is guaranteed to converge
    to the optimal cost-to-go, $\hat{J}^* \rightarrow J^*$
    <elib>Bertsekas96</elib>, and in practice does so rapidly. Typically the
    update is done in <em>batch</em> -- e.g. the estimate is updated for all $i$
    at once -- but the <em>asynchronous</em> version where states are updated
    one at a time is also known to converge, so long as every state is
    eventually updated infinitely often.  Assuming the graph has a goal state
    with a zero-cost self-transition, then this cost-to-go function represents
    the weighted shortest distance to the goal. </p>

    <p> Value iteration is an amazingly simple algorithm, but it accomplishes
    something quite amazing: it efficiently computes the long-term cost of an
    optimal policy from <i>every</i> state by iteratively evaluating the
    one-step cost.  If we know the optimal cost-to-go, then it's easy to extract
    the optimal policy, $a = \pi^*(s)$: \begin{equation} \pi^*(s_i) = \argmin_a
    \left[ g(s_i,a) + J^*\left( f(s_i,a) \right) \right].
    \label{eq:policy_update} \end{equation} It's a simple algorithm, but playing
    with an example can help our intuition.</p>

    <div data-type="example" class="drake"><h1>Grid World</h1>

      <p>Imagine a robot living in a grid (finite state) world.  Wants to get to
      the goal location.  Possibly has to negotiate cells with obstacles.  Actions
      are to move up, down, left, right, or do nothing.  <elib>Sutton98</elib></p>

      <figure>
        <a href="figures/gridworld_mintime.swf">
          <img width="80%" data-src="figures/gridworld_mintime.svg" />
        </a>
        <figcaption>The one-step cost for the grid-world minimum-time problem.
        The goal state has a cost of zero, the obstacles have a cost of 10, and
        every other state has a cost of 1. <em>Click the image to watch the
        value iteration algorithm in action.</em></figcaption>
      </figure>

      <todo>Port matlab example to pydrake</todo>

    </div> <!-- end grid world -->

    <todo>figure/text for graph approximation of a continuous state
    space.</todo>

    <div data-type="example" class="drake"><h1>Dynamic Programming for the Double Integrator</h1>

      <p>You can run value iteration for the double integrator (using
      barycentric interpolation to interpolate between nodes) in <drake></drake>
      using: </p>

      <pysrc>double_integrator/value_iteration.py</pysrc>

      <p>Please do take some time to try different cost functions by
      editing the code yourself.</p>

    </div>

    <p> Let's take a minute to appreciate how amazing this is.  Our solution to
    finding the optimal controller for the double integrator wasn't all that
    hard, but it required some mechanical intuition and solutions to
    differential equations.  The resulting policy was non-trivial -- bang-bang
    control with a parabolic switching surface. The value iteration algorithm
    doesn't use any of this directly -- it's a simple algorithm for graph
    search.  But remarkably, it can generate effectively the same policy with
    just a few moments of computation.</p>

    <p>It's important to note that there <em>are</em> some differences between
    the computed policy and the optimal policy that we derived, due to
    discretization errors.  We will ask you to explore these in the
    problems.</p>

    <p>The real value of this numerical solution, however, is unlike our
    analytical solution for the double integrator, we can apply this same
    algorithm to any number of dynamical systems virtually without modification.
    Let's apply it now to the simple pendulum, which was intractable
    analytically.</p>

    <div data-type="example" class="drake"><h1>Dynamic Programming for the
    Simple Pendulum</h1>

      <p>You can run value iteration for the simple pendulum (using barycentric
      interpolation to interpolate between nodes) in <drake></drake> using:</p>

      <pysrc>pendulum/value_iteration.py</pysrc>

    <p>Again, you can easily try different cost functions by
      editing the code yourself.</p>

    </div>

  </section> <!-- end of graph search -->

  <section data-type="sect1"><h1>Continuous dynamic programming</h1>

    <p> I find the graph search algorithm extremely satisfying as a first step,
    but also become quickly frustrated by the limitations of the discretization
    required to use it. In many cases, we can do better; coming up with
    algorithms which work more natively on continuous dynamical systems.  We'll
    explore those extensions in this section.</p>

    <section data-type="sect2"><h1>The Hamilton-Jacobi-Bellman Equation</h1>

      <p> It's important to understand that the value iteration equations,
      equations (\ref{eq:value_update}) and (\ref{eq:policy_update}), are more
      than just an algorithm.  They are also sufficient conditions for
      optimality: if we can produce a $J^*$ and $\pi^*$ which satisfy these
      equations, then $\pi^*$ must be an optimal controller.  There are an
      analogous set of conditions for the continuous systems.  For a system
      $\dot{\bx} = f(\bx,\bu)$ and an infinite-horizon additive cost
      $\int_0^\infty g(\bx,\bu)dt$, we have: \begin{gather} 0 = \min_\bu \left[
      g(\bx,\bu) + \pd{J^*}{\bx}f(\bx,\bu) \right], \label{eq:HJB} \\ \pi^*(\bx)
      = \argmin_\bu \left[ g(\bx,\bu) + \pd{J^*}{\bx}f(\bx,\bu) \right].
      \end{gather} Equation \ref{eq:HJB} is known as the
      <em>Hamilton-Jacobi-Bellman</em> (HJB) equation.  <elib>Bertsekas05</elib>
      <!-- chapter 3 --> gives an informal derivation of these equations as the
      limit of a discrete-time approximation of the dynamics, and also gives the
      following sufficiency theorem:</p>

      <div data-type="theorem"><h1>HJB Sufficiency Theorem</h1>

        <p>The following statement is adapted from Proposition 3.2.1 of
        <elib>Bertsekas05</elib>.  Consider a system $\dot{\bx}=f(\bx,\bu)$ and
        an infinite-horizon additive cost $\int_0^\infty g(\bx,\bu)dt$.  Suppose
        $J(\bx)$ is a solution to the HJB equation; that is $J$ is continuously
        differentiable in $\bx$ and is such that \[ 0 = \min_{u\in U} \left[
        g(\bx,\bu) + \pd{J}{\bx}f(\bx,\bu) \right],\quad \text{for all } \bx. \]
        Suppose also that $\pi^*(\bx)$ attains the minimum in the equation for
        all $\bx$.  Further assume that the differential equation described by
        $f$ has a unique solution starting from any state $\bx$, that the
        control input trajectory, $\bu^*(t)$, obtained by evaluating $\pi^*$
        along any solution is piecewise continuous as a function of $t$, and
        that there exists at least one fixed point, $\bx_{fp}$, with
        $f(\bx_{fp}, \pi^*(\bx_{fp})) = 0$ and $J^*(\bx_{fp}) = 0$. Then $J$ is
        equal to the optimal cost-to-go function, $J(\bx)=J^*(\bx)$ for all
        $\bx$, and the control trajectories $\bu^*(t)$ are optimal.</p>

      </div>

      <p>As a tool for verifying optimality, the HJB equations are actually
      surprisingly easy to work with: we can verify optimality for an
      infinite-horizon objective without doing any integration; we simply have
      to check a derivative condition on the optimal cost-to-go function $J^*$.
      Let's see this play out on the double integrator example.</p>

      <div data-type="example"><h1>HJB for the Double Integrator</h1>

        <p>Consider the problem of regulating the double integrator (this time
        without input limits) to the origin   using a quadratic cost: $$
        g(\bx,\bu) = q^2 + \dot{q}^2 + u^2. $$  I claim   (without derivation)
        that the optimal controller for this objective is $$\pi(\bx) = -q -
        \sqrt{3}\dot{q}.$$   To convince you that this is indeed optimal, I have
        produced   the following cost-to-go function: $$J(\bx) = \sqrt{3} q^2 +
        2 q \dot{q} + \sqrt{3} \dot{q}^2.$$</p>

        <p>Taking \begin{gather*} \pd{J}{q} = 2\sqrt{3} q + 2\dot{q}, \qquad
        \pd{J}{\dot{q}} = 2q + 2\sqrt{3}\dot{q}, \end{gather*}   we can write
        \begin{align*} g(\bx,\bu) + \pd{J}{\bx}f(\bx,\bu) &= q^2 + \dot{q}^2 +
        u^2 + (2\sqrt{3} q + 2\dot{q}) \dot{q} + (2q + 2\sqrt{3}\dot{q}) u
        \end{align*}   This is a convex quadratic function in $u$, so we can
        find the minimum with respect to $u$ by finding where the gradient with
        respect to $u$ evaluates to zero.   \[ \pd{}{u} \left[ g(\bx,\bu) +
        \pd{J}{\bx} f(\bx,\bu) \right] = 2u + 2q + 2\sqrt{3}\dot{q}. \]  Setting
        this equal to $0$ and   solving for $u$ yields: $$u^* = -q - \sqrt{3}
        \dot{q},$$ thereby confirming that our policy $\pi$ is in fact the
        minimizer.  Substituting $u^*$ back into the HJB reveals that the right
        side does in fact simplify to zero.  I hope you are convinced!</p>

      </div>

      <p>Note that evaluating the HJB for the time-to-go of the minimum-time
      problem for the double integrator will also reveal that the HJB is
      satisfied wherever that gradient is well-defined.  This is certainly
      mounting evidence in support of our bang-bang policy being optimal, but
      since $\pd{J}{\bx}$ is not defined everywhere, it does not actually
      satisfy the requirements of the sufficiency theorem as stated above.</p>

    </section> <!-- end HJB -->

    <section data-type="sect2" id="hjb_minimizing_control"><h1>Solving for the minimizing control</h1>

      <p>We still face a few barriers to actually using the HJB in an algorithm.
      The first barrier is the minimization over $u$.  When the action set was
      discrete, as in the graph search version, we could evaluate the one-step
      cost plus cost-to-go for every possible action, and then simply take the
      best.  For continuous action spaces, in general we cannot rely on the
      strategy of evaluating a finite number of possible $\bu$'s to find the
      minimizer.</p>

      <p>All is not lost.  In the quadratic cost double integrator example
      above, we were able   to solve explicitly for the minimizing $\bu$ in
      terms of the cost-to-go.  It turns out that   this strategy will actually
      work for a number of the problems we're interested in, even   when the
      system (which we are given) or cost function (which we are free to pick,
      but which   should be expressive) gets more complicated.</p>

      <p>Recall that I've already tried to convinced you that a majority of the
      systems of interest   are <em>control affine</em>, e.g. I can write \[
      f(\bx,\bu) = f_1(\bx) + f_2(\bx)\bu. \]   We can make another dramatic
      simplification by restricting ourselves to instantaneous cost functions of
      the form \[ g(\bx,\bu) = g_1(\bx) + \bu^T {\bf R} \bu, \qquad {\bf R}={\bf
      R}^T \succ 0. \] In my view, this is not very restrictive - many of the
      cost functions that I find myself choosing to write down can be expressed
      in this form.  Given these assumptions, we can write the HJB as \[ 0 =
      \min_{\bu} \left[ g_1(\bx) + \bu^T {\bf R} \bu + \pd{J}{\bx} \left[
      f_1(\bx) + f_2(\bx)\bu \right]\right]. \]  Since this is a positive
      quadratic function in $\bu$, if the system does not have any constraints
      on $\bu$, then we can solve in closed-form for the minimizing $\bu$ by
      taking the gradient of the right-hand side: \[ \pd{}{\bu} = 2\bu^T {\bf R}
      + \pd{J}{\bx} f_2(\bx) = 0, \] and setting it equal to zero to obtain \[
      \bu^* = -\frac{1}{2}{\bf R}^{-1}f_2^T(\bx) \pd{J}{\bx}^T.\] If there are
      linear constraints on the input, such as torque limits, then more
      generally this could be solved (at any particular $\bx$) as a quadratic
      program.</p>

      <p> What happens in the case where our system is not control affine or if
      we really   do need to specify an instantaneous cost function on $\bu$
      that is not simply quadratic?   If the goal is to produce an iterative
      algorithm, like value iteration, then one   common approach is to make a
      (positive-definite) quadratic approximation in $\bu$ of the HJB, and
      updating   that approximation on every iteration of the algorithm.  This
      broad approach is often referred to as <em>differential dynamic
      programming</em> (c.f. <elib>Jacobson70</elib>).   </p>

    </section> <!-- end solve for u -->

    <section data-type="sect2"><h1>Numerical solutions for $J^*$</h1>

      <p> The other major barrier to using the HJB in a value iteration
      algorithm is that  the estimated optimal cost-to-go function, $\hat{J}^*$,
      must somehow be represented with a finite set of numbers, but we don't yet
      know anything about the potential form it must take. In fact, knowing the
      time-to-goal solution for minimum-time problem with the double integrator,
      we see that this function might need to be non-smooth for even very simple
      dynamics and objectives.</p>

      <p>One natural way to parameterize $\hat{J}^*$ -- a scalar valued-function
      defined over the state space -- is to define the values on a mesh.  This
      approach then admits algorithms with close ties to the relatively very
      advanced numerical methods used to solve other partial differential
      equations (PDEs), such as the ones that appear in finite element modeling
      or fluid dynamics. One important difference, however, is that our PDE
      lives in the dimension of the state space, while many of the <a
      href="https://en.wikipedia.org/wiki/Types_of_mesh">mesh
      representations</a> from the other disciplines are optimized for two or
      three dimensional space.</p>

      <p>A slightly more general view of the problem would describe the mesh
      (and the associated interpolation functions) as just one form of
      representations for <a
      href="https://en.wikipedia.org/wiki/Function_approximation">function
      approximation</a>.</p>

      <todo>  (see Appendix C for a brief background on function approximation)
      </todo>

      <section data-type="sect3"><h1>Representing the cost-to-go with function
      approximation</h1>

        <p>If we approximate $J^*$ with a finitely-parameterized function
        $\hat{J}_\balpha^*$, with parameter vector $\balpha$, then   this
        immediately raises many important questions:</p>

        <ul>
          <li>What if the true cost-to-go function does not live in the
        prescribed function class; e.g., there does not exist an $\balpha$ which
        satisfies the sufficiency conditions for all $\bx$?  (This seems very
        likely to be the case.)</li>
          <li>What update should we apply in the iterative algorithm?</li>
          <li>What can we say about it's convergence?</li>
        </ul>

        <todo> add quote from neuro-dynamic programming preface </todo>

        <p>In general, many of these questions will have to go unanswered.  But
        there are some answers available for the special case of <em>linear
        function approximators</em>.  A linear function approximator takes the
        form: \[ \hat{J}^*_\balpha(\bx) = \sum_i \alpha_i \psi_i(\bx) =
        \balpha^T \bpsi(\bx), \] where $\bpsi(\bx)$ is a vector of potentially
        nonlinear features.  Common examples of features include polynomials,
        radial basis functions, or most interpolation schemes used on a mesh.
        The distinguishing feature of a linear function approximator is the
        ability to exactly solve for $\balpha$ in order to represent a desired
        function optimally, in a least-squares sense.  For instance, if we have
        a desired set of $K$ input-output points, $(\bx_k,J^d_k)$, then we can
        write down the closed-form solution to \[ \minimize_\balpha \sum_k
        \left(\hat{J}_\balpha(\bx_k) - J^d_k \right)^2. \] Using the least
        squares solution in a value iteration update is sometimes referred to as
        <i>fitted value iteration</i>, where $\bx_k$ are some number of samples
        taken from the continuous space and the iteration becomes
        \begin{gather*} J^d_k = \min_\bu \left[ g(\bx_k,\bu) +
        \hat{J}_\alpha\left({f(\bx_k,\bu)}\right) \right], \\ \balpha \Leftarrow
        \argmin_\balpha \sum_k \left(\hat{J}_\balpha(\bx_k) - J^d_k \right)^2.
        \end{gather*} Remarkably, for linear function approximators, this update
        is still known to converge to the globally optimal $\balpha^*$.</p>

        <!-- Andrew also has fitted value iteration notes:
        http://cs229.stanford.edu/notes/cs229-notes12.pdf -->

        <todo> add citations for convergence results </todo>
      </section>

      <section data-type="sect3"><h1>Representing the cost-to-go on a mesh</h1>

        <p>Imagine that we use a mesh to approximate the cost-to-go function
        over that state space with $K$ mesh points $\bx_k$. We would like to
        perform the value iteration update: \begin{equation} \forall k,
        \hat{J}^*(\bx_k) \Leftarrow \min_\bu \left[ g(\bx_k,\bu) +
        \hat{J}^*\left({f(\bx_k,\bu)}\right) \right],
        \label{eq:mesh_value_iteration} \end{equation} but must deal with the
        fact that $f(\bx_k,\bu)$ might not result in a next state that is
        directly at a mesh point.  Most interpolation schemes for a mesh can be
        written as some weighted combination of the values at nearby mesh
        points, e.g. \[ \hat{J}^*(\bx) = \sum_i \beta_i(\bx) \hat{J}^*(\bx_i),
        \quad \sum_i \beta_i = 1 \] with $\beta_i$ the relative weight of the
        $i$th mesh point.  In <drake></drake> we have implemented barycentric
        interpolation<elib>Munos98</elib>. Taking $\alpha_i = \hat{J}^*(\bx_i)$,
        the cost-to-go estimate at mesh point $i$, we can see that this is
        precisely an important special case of fitted value iteration with
        linear function approximation.  Furthermore, assuming $\beta_i(\bx_i) =
        1,$ (e.g., the only point contributing to the interpolation <i>at a mesh
        point</i> is the value at the mesh point itself), the update in Eq.
        (\ref{eq:mesh_value_iteration}) is precisely the least-squares update
        (and it achieves zero error). </p>


      </section>

    </section> <!-- end function approx -->

  </section> <!-- end continuous time DP -->

  <todo> try just running snopt on quartic objective </todo>

  <!--
  <section data-type="sect2"><h1>A continuous policy iteration algorithm</h1>

  <todo> simple (e.g. one-d example) </todo>

  </section> -->
  <!-- end policy iteration -->

  <!--
  <section data-type="sect2"><h1>How far can we take this?  (Performance and Scaling)</h1>

  <todo>
  Errors in bang-bang for double integrator due to discretization.

  Limited in number of dimensions.  Function approximation, but lacks convergence results.
  </todo>

  </section> --><!-- end scaling -->

  <section data-type="sect1"><h1>Stochastic Optimal Control</h1>

    <p> One of the most amazing features of the dynamic programming, additive
    cost approach to optimal control is the relative ease with which it extends
    to optimizing stochastic systems. </p>

    <section data-type="sect2"><h1>Graph search -- stochastic shortest path
    problems</h1>

      <p>For discrete systems, we can generalize our dynamics on a graph by
      adding action-dependent transition probabilities to the edges.  This new
      dynamical system is known as a Markov Decision Process (MDP), and we write
      the dynamics completely in terms of the transition probabilities \[
      \Pr(s[n+1] = s' | s[n] = s, a[n] = a). \] For discrete systems, this is
      simply a big lookup table.  The cost that we incur for any execution of
      system is now a random variable, and so we formulate the goal of control
      as optimizing the expected cost, e.g. \begin{equation} J^*(s[0]) =
      \min_{a[\cdot]} E \left[ \sum_{n=0}^\infty g(s[n],a[n]) \right].
      \label{eq:stochastic_dp_optimality_cond} \end{equation}  Note that there
      are many other potential objectives, such as minimizing the worst-case
      error, but the expected cost is special because it preserves the dynamic
      programming recursion: \[ J^*(s) = \min_a E \left[ g(s,a) + J^*(s')\right]
      = \min_a \left[ g(s,a) + \sum_{s'} \Pr(s'|s,a) J^*(s') \right].\]
      Remarkably, if we use these optimality conditions to construct our value
      iteration algorithm \[ \hat{J}(s) \Leftarrow \min_a \left[ g(s,a) +
      \sum_{s'} \Pr(s'|s,a) \hat{J}(s') \right],\] then this algorithm has the
      same strong convergence guarantees of its counterpart for deterministic
      systems.  And it is essentially no more expensive to compute!</p>

    </section>

    <section data-type="sect2"><h1>Stochastic interpretation of deterministic,
    continuous-state value iteration</h1>

      <p> There is a particularly cute observation to be made here. Let's assume
      that we have discrete control inputs and discrete-time dynamics, but a
      continuous state space.  Recall the fitted value iteration on a mesh
      algorithm described above. In fact, the resulting update is exactly the
      same as if we treated the system as a discrete state MDP with $\beta_i$
      representing the probability of transitioning to state $x_i$! This sheds
      some light on the impact of discretization on the solutions --
      discretization error here will cause a sort of diffusion corresponding to
      the probability of spreading across neighboring nodes.</p>

    </section>

  </section> <!-- end of stochastic optimal control -->

  <section data-type="sect1"><h1>Extensions</h1>

    <todo> add section on extensions.  discounting.  finite-horizon / time-varying dynamics or cost.</todo>

    <section data-type="sect2"><h1>Linear Programming Approach</h1>

      <p>For discrete MDPs, value iteration is a magical algorithm because it is
      simple, but known to converge to the global optimal, $J^*$.  However,
      other important algorithms are known; one of the most important is a
      solution approach using linear programming.  This formulation provides an
      alternative view, but may also be more generalizable and even more
      efficient for some instances of the problem.</p>

      <p>Recall the optimality conditions from Eq.
      (\ref{eq:stochastic_dp_optimality_cond}).  If we describe the cost-to-go
      function as a vector, $J_i = J(s_i)$, then these optimality conditions can
      be rewritten in vector form as \begin{equation} \bJ = \min_a \left[ \bg(a)
      + \bT(a) \bJ \right], \label{eq:vector_stochastic_dp} \end{equation} where
      $g_i(a) = g(s_i,a)$ is the cost vector, and $T_{i,j}(a) = \Pr(s_j|s_i,a)$
      is the transition probability matrix.  Let us take $\bJ$ as the vector of
      decision variables, and replace Eq. (\ref{eq:vector_stochastic_dp}) with
      the constraints: \begin{equation} \forall a, \bJ \le \bg(a) + \bT(a)
      \bJ.\end{equation}  Clearly, for finite $a$, this is finite list of linear
      constraints, and for any vector $\bJ$ satisfying these constraints we have
      $\bJ \le \bJ^*$ (elementwise).  Now write the linear program:
      \begin{gather*} \maximize_\bJ \quad \bc^T \bJ, \\ \subjto \quad \forall a,
      \bJ \le
      \bg(a) + \bT(a) \bJ, \end{gather*} where $c$ is any positive vector.  The
      solution to this problem is $\bJ = \bJ^*$.</p>

      <p>Perhaps even more interesting is that this approach can be generalized
      to linear function approximators.  Taking a vector form of my notation
      above, now we have a matrix of features with $\bPsi_{i,j} =
      \psi^T_j(\bx_i)$ and we can write the LP \begin{gather} \maximize_\balpha
      \quad \bc^T \bPsi \balpha, \\ \subjto \quad \forall a, \bPsi \balpha \le
      \bg(a) + \bT(a) \bPsi \balpha. \end{gather}  This time the solution is not
      necessarily optimal, because $\bPsi \balpha^*$ only approximates $\bJ^*$,
      and the relative values of the elements of $\bc$ (called the
      "state-relevance weights") can determine the relative tightness of the
      approximation for different features <elib>Farias02</elib>.</p>

    </section>

  </section> <!-- end of extensions -->

</section> <!-- ***************  end of dynamic programming **************   -->


<section data-type="chapter" class="chapter" id="lqr"><h1>Linear Quadratic Regulators</h1>

  <p>While solving the dynamic programming problem for continuous systems is
  very hard in general, there are a few very important special cases where the
  solutions are very accessible. Most of these involve variants on the case of
  linear dynamics and quadratic cost.  The simplest case, called the linear
  quadratic regulator (LQR), is formulated as stabilizing a time-invariant
  linear system to the origin.</p>

  <p>The linear quadratic regulator is likely the most important and influential
  result in optimal control theory to date.  In this chapter we will derive the
  basic algorithm and a variety of useful extensions.</p>

  <section data-type="sect1"><h1>Basic Derivation</h1>

    <p>Consider a linear time-invariant system in state-space form, $$\dot{\bx}
    = {\bf A}\bx + \bB\bu,$$ with the infinite-horizon cost function given by
    $$J = \int_0^\infty \left[ \bx^T {\bf Q} \bx + \bu^T {\bf R} \bu \right] dt,
    \quad {\bf Q} = {\bf Q}^T \ge {\bf 0}, {\bf R} = {\bf R}^T > 0.$$  Our goal
    is to find the optimal cost-to-go function $J^*(\bx)$ which satisfies the
    HJB: $$\forall \bx, \quad 0 = \min_\bu \left[ \bx^T {\bf Q} \bx + \bu^T {\bf
    R} \bu + \pd{J^*}{\bx} \left( {\bf A}\bx + \bB\bu \right) \right].$$</p>

    <p> There is one important step here -- it is well known that for this
    problem the optimal cost-to-go function is quadratic.  This is easy to
    verify. Let us choose the form: $$J^*(\bx) = \bx^T {\bf S} \bx, \quad {\bf
    S} = {\bf S}^T \succeq 0.$$ The gradient of this function is $$\pd{J^*}{\bx}
      = 2 \bx^T {\bf S}.$$  </p>

    <p> Since we have guaranteed, by construction, that the terms inside the
    $\min$ are quadratic and convex (because ${\bf R} \succ 0$), we can take the
    minimum explicitly by finding the solution where the gradient of those terms
    vanishes: $$\pd{}{\bu} = 2\bu^T {\bf R} + 2 \bx^T {\bf S} \bB = 0.$$ This
    yields the optimal policy $$\bu^* = \pi^*(\bx) = - {\bf R}^{-1} \bB^T {\bf
    S} \bx = - \bK \bx.$$</p>

    <p>Inserting this back into the HJB and simplifying yields $$0 = \bx^T
    \left[ {\bf Q} - {\bf S B R}^{-1}\bB^T{\bf S} + 2{\bf SA} \right]\bx.$$ All
    of the terms here are symmetric except for the $2{\bf SA}$, but since
    $\bx^T{\bf SA}\bx = \bx^T{\bf A}^T{\bf S}\bx$, we can write $$0 = \bx^T
    \left[ {\bf Q} - {\bf S B R}^{-1}\bB^T{\bf S} + {\bf SA} + {\bf A}^T{\bf S}
    \right]\bx.$$ and since this condition must hold for all $\bx$, it is
    sufficient to consider the matrix equation $$0 = {\bf S} {\bf A} + {\bf A}^T
    {\bf S} - {\bf S} \bB {\bf R}^{-1} \bB^T {\bf S} + {\bf Q}.$$ This extremely
    important equation is a version of the <em>algebraic Riccati equation</em>.
    Note that it is quadratic in ${\bf S}$, making its solution non-trivial, but
    it is well known that the equation has a single positive-definite solution
    if and only if the system is controllable and there are good numerical
    methods for finding that solution, even in high-dimensional problems.  Both
    the optimal policy and optimal cost-to-go function are available from
    <drake></drake> by calling <code> (K,S) =
    LinearQuadraticRegulator(A,B,Q,R)</code>.</p>

    <p>If the appearance of the quadratic form of the cost-to-go seemed
    mysterious, consider that the solution to the linear system $\dot\bx = (\bA
    - \bB\bK)\bx$ takes the form $\bx(t) = e^{(\bA-\bB\bK)t}\bx(0)$, and try
    inserting this back into the integral cost function.</p>

    <p>It is worth examining the form of the optimal policy more closely. Since
    the value function represents cost-to-go, it would be sensible to move down
    this landscape as quickly as possible.  Indeed, $-{\bf S}\bx$ is in the
    direction of steepest descent of the value function. However, not all
    directions are possible to achieve in state-space. $-\bB^T {\bf S} \bx$
    represents precisely the projection of the steepest descent onto the control
    space, and is the steepest descent achievable with the control inputs $\bu$.
    Finally, the pre-scaling by the matrix ${\bf R}^{-1}$ biases the direction
    of descent to account for relative weightings that we have placed on the
    different control inputs.  Note that although this interpretation is
    straight-forward, the slope that we are descending (in the value function,
    ${\bf S}$) is a complicated function of the dynamics and cost.</p>

    <!--<div data-type="example"><h1>LQR for the Double Integrator</h1>

    <p>Let's use the LQR result to solve for the optimal regulator of our
    trivial linear system: $$\ddot{q} = u.$$</p>
    </div>-->

    <section data-type="sect2"><h1>Local stabilization of nonlinear systems</h1>

      <p>LQR is extremely relevant to us even though our primary interest is in
      nonlinear dynamics, because it can provide a local approximation of the
      optimal control solution for the nonlinear system.  Given the nonlinear
      system $\dot{\bx} = f(\bx,\bu)$, and a stabilizable operating point,
      $(\bx_0,\bu_0)$, with $f(\bx_0,\bu_0) = 0.$  We can define a relative
      coordinate system $$\bar\bx = \bx - \bx_0, \quad \bar\bu = \bu - \bu_0,$$
      and observe that $$\dot{\bar\bx} = \dot{\bx} = f(\bx,\bu),$$ which we can
      approximate with a first-order Taylor expansion to $$\dot{\bar\bx} \approx
      f(\bx_0,\bu_0) + \pd{f(\bx_0,\bu_0)}{\bx} (\bx - \bx_0) +
      \pd{f(\bx_0,\bu_0)}{\bu} (\bu - \bu_0) = {\bf A}\bar{\bx} +
      \bB\bar\bu.$$</p>

      <p>Similarly, we can define a quadratic cost function in the error
      coordinates, or take a (positive-definite) second-order approximation of a
      nonlinear cost function about the operating point (linear and constant
      terms in the cost function can be easily incorporated into the derivation
      by parameterizing a full quadratic form for $J^*$, as seen in the Linear
      Quadratic Tracking derivation below).</p>

      <p>The resulting controller takes the form $\bar\bu^* = -{\bf K}\bar\bx$
      or $$\bu^* = \bu_0 - {\bf K} (\bx - \bx_0).$$  For convenience,
      <drake></drake> allows you to call <code>controller =
      LinearQuadraticRegulator(system, context, Q, R)</code> on most dynamical
      systems (including block diagrams built up of many subsystems); it will
      perform the linearization for you.</p>

    </section>

    <todo> add discrete time derivation? </todo>

  </section> <!-- end basic derivation -->

  <section data-type="sect1"><h1>Finite-horizon formulations</h1>

    <p>Recall that the cost-to-go for finite-horizon problems is time-dependent,
    and therefore the HJB sufficiency condition requires an additional term for
    $\pd{J^*}{t}$. $$ \forall \bx, \forall t\in[t_0,t_f],\quad 0 = \min_\bu
    \left[ g(\bx,\bu) + \pd{J^*}{\bx}f(\bx,\bu) + \pd{J^*}{t} \right]. $$</p>

    <section data-type="sect2"><h1>Finite-horizon LQR</h1>

      <p> Consider systems governed by an LTI state-space equation of the form
      $$\dot{\bx} = {\bf A}\bx + \bB\bu,$$ and a finite-horizon cost function
      with

      \begin{gather*}
      h(\bx) = \bx^T {\bf Q}_f \bx,\quad {\bf Q}_f = {\bf Q}_f^T \ge {\bf 0}
      \\ g(\bx,\bu,t) = \bx^T {\bf Q} \bx + \bu^T {\bf R} \bu, \quad {\bf Q} =
         {\bf Q}^T \ge 0, {\bf R}={\bf R}^T>0
      \end{gather*}

      Writing the HJB, we have $$ 0 = \min_\bu \left[\bx^T {\bf Q} \bx + \bu^T
      {\bf R}\bu + \pd{J^*}{\bx} \left({\bf A} \bx + \bB \bu \right) +
      \pd{J^*}{t} \right]. $$  Due to the positive definite quadratic form on
      $\bu$, we can find the minimum by setting the gradient to zero:

      \begin{gather*}
      \pd{}{\bu} = 2 \bu^T {\bf R} + \pd{J^*}{\bx} \bB  = 0 \\
      \bu^* = \pi^*(\bx,t) = - \frac{1}{2}{\bf R}^{-1} \bB^T \pd{J^*}{\bx}^T
      \end{gather*}

      In order to proceed, we need to investigate a particular form for the
      cost-to-go function, $J^*(\bx,t)$.  Let's try a solution of the form:
      $$J^*(\bx,t) = \bx^T {\bf S}(t) \bx, \quad {\bf S}(t) = {\bf S}^T(t) >
      {\bf 0}.$$ In this case we have $$\pd{J^*}{\bx} = 2 \bx^T {\bf S}(t),
      \quad \pd{J*}{t} = \bx^T \dot{\bf S}(t) \bx,$$ and therefore

      \begin{gather*} \bu^* = \pi^*(\bx,t) = - {\bf R}^{-1} \bB^T {\bf S}(t) \bx
      \\ 0 = \bx^T \left[ {\bf Q} - {\bf S}(t) \bB {\bf R}^{-1} \bB^T {\bf S}(t)
      + {\bf S}(t) {\bf A} + {\bf A}^T {\bf S}(t) + \dot{\bf S}(t) \right]
      \bx.\end{gather*}

      Therefore, ${\bf S}(t)$ must satisfy the condition (known as the
      continuous-time <em>differential Riccati equation</em>): $$-\dot{\bf S}(t)
      = {\bf S}(t) {\bf A} + {\bf A}^T {\bf S}(t) - {\bf S}(t) \bB {\bf R}^{-1}
      \bB^T {\bf S}(t) + {\bf Q},$$ and the terminal condition $${\bf S}(T) =
      {\bf Q}_f.$$ Since we were able to satisfy the HJB with the minimizing
      policy, we have met the sufficiency condition, and have found the optimal
      policy and optimal cost-to-go function.</p>

      <p>Note that the infinite-horizon LQR solution described in the prequel is
      exactly the steady-state solution of this equation, defined by $\dot{\bf
      S}(t) = 0$. Indeed, for controllable systems this equation is stable
      (backwards in time), and as expected the finite-horizon solution converges
      on the infinite-horizon solution as the horizon time limits to infinity.
      </p>

    </section>

    <section data-type="sect2"><h1>Time-varying LQR</h1>

      <p>The derivation above holds even if the dynamics are given by
      $$\dot{\bx} = {\bf A}(t)\bx + {\bf B(t)}\bu.$$  Similarly, the cost
      functions ${\bf Q}$ and ${\bf R}$ can also be time-varying.  This is
      quite surprising, as the class of time-varying linear systems is a quite
      general class of systems. It requires essentially no assumptions on how
      the time-dependence enters, except perhaps that if ${\bf A}$ or $\bB$ is
      discontinuous in time then one would have to use the proper techniques to
      accurately integrate the differential equation. As we will see in the <a
      href="underactuated.html?chapter=trajopt">chapter on trajectory
      optimization</a>, one of the most powerful applications involves
      linearizing around a nominal trajectory of a nonlinear system and using
      LQR to provide a trajectory controller.</p>

    </section>

    <section data-type="sect2"><h1>Linear Quadratic Optimal Tracking</h1>

      <p>For completeness, we consider a slightly more general form of the
      linear quadratic regulator.  The standard LQR derivation attempts to drive
      the system to zero.  Consider now the problem:

      \begin{gather*} \dot{\bx} = {\bf A}\bx + \bB\bu \\ h(\bx) = (\bx -
      \bx^d(T))^T {\bf Q}_f (\bx - \bx^d(T)), \quad {\bf Q}_f = {\bf Q}_f^T \ge
      0 \\ g(\bx,\bu,t) = (\bx - \bx^d(t))^T {\bf Q} (\bx - \bx^d(t)) + (\bu -
      \bu^d(t))^T {\bf R} (\bu - \bu^d(t)),\\ {\bf Q} = {\bf Q}^T \ge 0, {\bf
      R}={\bf R}^T>0 \end{gather*}

      Now, guess a solution

      \begin{gather*} J^*(\bx,t) = \bx^T {\bf S_2}(t) \bx + \bx^T {\bf s_1}(t) +
      s_0(t), \quad {\bf S_2}(t) = {\bf S_2}^T(t) > {\bf 0}. \end{gather*}

      In this case, we have $$\pd{J^*}{\bx} = 2 \bx^T {\bf S_2}(t) + {\bf
      s_1}^T(t),\quad \pd{J^*}{t} = \bx^T \dot{\bf S_2}(t) \bx + \bx^T \dot{\bf
      s_1}(t) + \dot{s_0}(t).$$ Using the HJB, $$ 0 = \min_\bu \left[(\bx -
      \bx^d(t))^T {\bf Q} (\bx - \bx^d(t)) + (\bu - \bu^d(t))^T {\bf R} (\bu -
      \bu^d(t)) + \pd{J^*}{\bx} \left({\bf A} \bx + \bB \bu \right) +
      \pd{J^*}{t} \right], $$ we have

      \begin{gather*} \pd{}{\bu} = 2 (\bu - \bu^d(t))^T{\bf R} + (2\bx^T{\bf
      S_2}(t) + {\bf s_1}^T(t))\bB = 0,\\ \bu^*(t) =  \bu^d(t) - {\bf R}^{-1}
      \bB^T\left[{\bf S_2}(t)\bx + \frac{1}{2}{\bf s_1}(t)\right] \end{gather*}

      The HJB can be satisfied by integrating backwards

      \begin{align*}
      -\dot{\bf S_2}(t) =& {\bf Q} - {\bf S_2}(t) \bB {\bf R}^{-1} {\bf
      B}^T {\bf S_2}(t) + {\bf S_2}(t) {\bf A} + {\bf A}^T {\bf S_2}(t)\\
      -\dot{\bf s_1}(t) =& -2 {\bf Q} \bx^d(t) + \left[{\bf A}^T - {\bf S}_2
        \bB {\bf R}^{-1} \bB^T \right]{\bf s_1}(t) + 2{\bf
        S_2}(t) \bB \bu^d(t)\\
      -\dot{s_0}(t) =&  \bx^d(t)^T {\bf Q} \bx^d(t) - \frac{1}{4} {\bf
        s_1}^T(t) \bB {\bf R}^{-1} \bB^T {\bf s_1}(t) + {\bf
        s_1}(t)^T \bB \bu^d(t),
      \end{align*}

      from the final conditions

      \begin{align*}
      {\bf S_2}(T) =& {\bf Q}_f\\
      {\bf s_1}(T) =& -2 {\bf Q}_f \bx^d(T) \\
      s_0(T) =& \left[\bx^d(T)\right]^T {\bf Q}_f \left[ \bx^d(T) \right].
      \end{align*}

      Notice that the solution for ${\bf S_2}$ is the same as the simpler LQR
      derivation, and is symmetric (as we assumed).  Note also that $s_0(t)$ has
      no effect on control (even indirectly), and so can often be ignored.</p>

      <p>A quick observation about the quadratic form, which might be helpful in
      debugging.  We know that $J(x,t)$ must be uniformly positive.  This is
      true iff ${\bf S}_2>0$ and $s_0 > \frac{1}{4} {\bf s}_1^T {\bf S}_2^{-1}
      {\bf s}_1$, which comes from evaluating the function at $x_{min}$ defined
      by $\pd{}{x} = 0$.</p>

      <todo>test this on an example, get my notation consistent (s(t)^T vs
      s^T(t), etc.</todo>
    </section>

    <!--
    <section data-type="sect2"><h1>Minimum-time LQR</h1>

      \begin{gather*}
      \dot{\bx} = {\bf A}\bx + \bB\bu \\
      h(\bx) = \bx^T {\bf Q}_f \bx,
      \quad {\bf Q}_f = {\bf Q}_f^T \ge 0 \\
      g(\bx,\bu,t) = 1 + \bx^T {\bf Q} \bx + \bu {\bf R} \bu,\\
      {\bf Q} = {\bf
      Q}^T \ge 0, {\bf R}={\bf R}^T>0
      \end{gather*}

    </section>
    -->

    <section data-type="sect2"><h1>Linear Final Boundary Value Problems</h1>

      <p> The finite-horizon LQR formulation can be used to impose a strict
      final boundary value condition by setting an infinite ${\bf Q}_f$.
      However, integrating the Riccati equation backwards from an infinite
      initial condition isn't very practical.  To get around this, let us
      consider solving for ${\bf P}(t) = {\bf S}(t)^{-1}$.  Using the matrix
      relation $\frac{d {\bf S}^{-1}}{dt} = - {\bf S}^{-1} \frac{d {\bf S}}{dt}
      {\bf S}^{-1}$, we have: $$-\dot{\bf P}(t) = - {\bf P}(t){\bf Q P}(t) +
      {\bf B R}^{-1} \bB - {\bf A P}(t) - {\bf P}(t){\bf A}^T,$$ with the final
      conditions $${\bf P}(T) = 0.$$ This Riccati equation can be integrated
      backwards in time for a solution.</p>

      <p> It is very interesting, and powerful, to note that, if one chooses
      ${\bf Q} = 0$, therefore imposing no position cost on the trajectory
      before time $T$, then this inverse Riccati equation becomes a linear ODE
      which can be solved explicitly. <todo>% add explicit solution here</todo>
      These relationships are used in the derivation of the controllability
      Grammian, but here we use them to design a feedback law.</p>

    </section>

  </section> <!-- end finite horizon -->

</section> <!-- ***************  end of LQR **************   -->

<section data-type="chapter" class="chapter" id="lyapunov"><h1>Lyapunov
  Analysis</h1>

  <p>Optimal control provides a powerful framework for formulating control
  problems using the language of optimization.  But solving optimal control
  problems for nonlinear systems is hard!  In many cases, we don't really care
  about finding the <em>optimal</em> controller, but would be satisfied with any
  controller that is guaranteed to accomplish the specified task.  In many
  cases, we still formulate these problems using computational tools from
  optimization, and in this chapter we'll learn about tools that can provide
  guaranteed control solutions for systems that are beyond the complexity for
  which we can find the optimal feedback.</p>

  <p>There are many excellent books on Lyapunov analysis; for instance
  <elib>Slotine90</elib> is an excellent and very readable reference and
  <elib>Khalil01</elib> can provide a rigorous treatment.  In this chapter I
  will summarize (without proof) some of the key theorems from Lyapunov
  analysis, but then will also introduce a number of numerical algorithms...
  many of which are new enough that they have not yet appeared in any mainstream
  textbooks.</p>

  <section data-type="sect1"><h1>Lyapunov Functions</h1>

    <p>Let's start with our favorite simple example. </p>

    <div data-type="example"><h1>Stability of the Damped Pendulum</h1>

      <center><img width="25%" data-src="figures/simple_pend.svg"/></center>

      <p>Recall that the equations of motion of the damped simple pendulum are
      given by \[ ml^2 \ddot{\theta} + mgl\sin\theta = -b\dot{\theta}, \] which
      I've written with the damping on the right-hand side to remind us that it
      is an external torque that we've modeled.</p>

      <p>These equations represent a simple second-order differential equation;
      in chapter 2 we discussed at some length what was known about the
      solutions to this differential equation--even without damping the equation
      for $\theta(t)$ as a function of $\theta(0)$, $\dot{\theta}(0)$, involves
      elliptic integrals of the first kind, and with damping we don't even have
      that.  Since we couldn't provide a solution analytically, in chapter 2 we
      resorted to a graphical analysis, and confirmed the intuition that there
      are fixed points in the system (at $\theta = k\pi$ for every integer $k$)
      and that the fixed points at $\theta = 2\pi k$ are asymptotically stable
      with a large basin of attraction.  The graphical analysis gave us this
      intuition, but can we actually prove this stability property?  In a way
      that might also work for much more complicated systems?</p>

      <p>One route forward was from looking at the total system energy (kinetic
      + potential), which we can write down: \[ E(\theta,\dot{\theta}) =
      \frac{1}{2} ml^2\dot{\theta}^2 - mgl \cos\theta. \] Recall that the
      contours of this energy function are the orbits of the undamped
      pendulum.</p>

      <center><img width="50%" data-src="figures/pend_undamped_phase.svg"/></center>

      <p>A natural route to proving the stability of the downward fixed points
      is by arguing that energy decreases for the damped pendulum (with $b>0$)
      and so the system will eventually come to rest at the minimum energy, $E =
      -mgl$, which happens at $\theta=2\pi k$.  Let's make that argument
      slightly more precise. </p>

      <p>Evaluating the time derivative of the energy reveals \[ \frac{d}{dt} E
      = - b\dot\theta^2 \le 0. \] This is sufficient to demonstrate that the
      energy will never increase, but it doesn't actually prove that the energy
      will converge to the minimum.  To take that extra step, we must observe
      that for most of the states with $\dot{E} = 0$, which for $b>0$ is the
      manifold where $\dot\theta=0$, the system can't actually remain in that
      state, but will rather pass through and enter another region where it
      continues to lose energy.  In fact, the fixed points are the only subset
      of this $\dot{E}=0$ manifold where the system can stay on the manifold.
      Therefore, unless the system is at a fixed point, it will continue to
      dissipate energy.  And therefore we can conclude that as $t\rightarrow
      \infty$, the system will indeed come to rest at a fixed point (though it
      could be any fixed point with an energy less than or equal to the initial
      energy in the system, $E(0)$).</p>

    </div>

    <p>This is an important example.  It demonstrated that we could use a
    relatively simple function, the system energy, to describe something about
    the long-term dynamics of the pendulum even though the actual trajectories
    of the system are (analytically) very complex.  It also demonstrated one of
    the subtleties of using an energy-like function that is non-increasing
    (instead of strictly decreasing) to prove asymptotic stability.</p>

    <p>Lyapunov functions generalize this notion of an energy function to more
    general systems, which might not be stable in the sense of some mechanical
    energy.  If I can find any positive function, call it $V(\bx)$, that gets
    smaller over time as the system evolves, then I can potentially use $V$ to
    make a statement about the long-term behavior of the system.  $V$ is called
    a <em>Lyapunov function</em>. </p>

    <p>Recall that we defined three separate notions for stability of a
    fixed-point of a nonlinear system: stability i.s.L., asymptotic stability,
    and exponential stability.  We can use Lyapunov functions to demonstrate
    each of these, in turn.</p>

    <div data-type="theorem"><h1>Lyapunov's Direct Method</h1>

      <p>Given a system $\dot{\bx} = f(\bx)$, with $f$ continuous, and for some
      region ${\cal B}$ around the origin (specifically an open subset of
      $\mathbf{R}^n$ containing the origin), if I can produce a scalar,
      continuously-differentiable function $V(\bx)$, such that \begin{gather*}
      V(\bx) > 0, \forall \bx \in {\cal B} \ne 0 \quad V(0) = 0, \text{ and} \\
      \dot{V}(\bx) = \pd{V}{\bx} f(\bx) \le 0, \forall \bx \in {\cal B} \ne 0
      \quad \dot{V}(0) = 0, \end{gather*} then the origin $(\bx = 0)$ is stable
      in the sense of Lyapunov (i.s.L.). </p>

      <p>If, additionally, we have $$\dot{V}(\bx) = \pd{V}{\bx} f(\bx) < 0,
      \forall \bx \in {\cal B} \ne 0,$$ then the origin is (locally)
      asymptotically stable.  And if we have $$\dot{V}(\bx) = \pd{V}{\bx} f(\bx)
      \le -\alpha V(x), \forall \bx \in {\cal B} \ne 0,$$ for some $\alpha>0$,
      then the origin is (locally) exponentially stable. </p>

    </div>

    <p>Note that for the sequel we will use the notation $V \succ 0$ to denote a
    <em>positive-definite function</em>, meaning that $V(0)=0$ and $V(\bx)>0$
    for all $\bx\ne0$ (and also $V \succeq 0$ for positive semi-definite, $V
    \prec 0$ for negative-definite functions).</p>

    <p>The intuition here is exactly the same as for the energy argument we made
    in the pendulum example:  since $\dot{V}(x)$ is always zero or negative, the
    value of $V(x)$ will only get smaller (or stay the same) as time progresses.
    Inside the subset ${\cal B}$, for every $\epsilon$-ball, I can choose a
    $\delta$ such that $|x(0)|^2 < \delta \Rightarrow |x(t)|^2 < \epsilon,
    \forall t$ by choosing $\delta$ sufficiently small so that the sublevel-set
    of $V(x)$ for the largest value that $V(x)$ takes in the $\delta$ ball is
    completely contained in the $\epsilon$ ball. Since the value of $V$ can only
    get smaller (or stay constant) in time, this gives stability i.s.L.. If
    $\dot{V}$ is strictly negative away from the origin, then it must eventually
    get to the origin (asymptotic stability). The exponential condition is
    implied by the fact that $\forall t>0, V(\bx(t)) \le V(\bx(0)) e^{-\alpha
    t}$.</p>

    <p> Notice that the system analyzed above, $\dot{\bx}=f(\bx)$, did not have
    any control inputs. Therefore, Lyapunov analysis is used to study either the
    passive dynamics of a system or the dynamics of a closed-loop system (system
    + control in feedback).  We will see generalizations of the Lyapunov
    functions to input-output systems later in the text. </p>

    <section data-type="sect2"><h1>Global Stability</h1>

      <p>The notion of a fixed point being stable i.s.L. is inherently a local
      notion of stability (defined with $\epsilon$- and $\delta$- balls around
      the origin), but the notions of asymptotic and exponential stability can
      be applied globally.  The Lyapunov theorems work for this case, too, with
      only minor modification.</p>

      <div data-type="theorem"><h1>Lyapunov analysis for global stability</h1>

        <p>Given a system $\dot{\bx} = f(\bx)$, with $f$ continuous, if I can
        produce a scalar, continuously-differentiable function $V(\bx)$, such
        that \begin{gather*} V(\bx) \succ 0, \\ \dot{V}(\bx) = \pd{V}{\bx}
        f(\bx) \prec 0, \text{ and} \\ V(\bx) \rightarrow \infty \text{ whenever
        } ||x||\rightarrow \infty,\end{gather*} then the origin $(\bx = 0)$ is
        globally asymptotically stable (G.A.S.).</p>

        <p>If additionally we have that $$\dot{V}(\bx) \preceq -\alpha V(\bx),$$
        for some $\alpha>0$, then the origin is globally exponentially
        stable.</p>

      </div>

      <p>The new condition, on the behavior as $||\bx|| \rightarrow \infty$ is
      known as "radially unbounded", and is required to make sure that
      trajectories cannot diverge to infinity even as $V$ decreases; it is only
      required for global stability analysis.</p>

    </section> <!-- global stability -->

    <section data-type="sect2"><h1>LaSalle's Invariance Principle</h1>

      <p>Perhaps you noticed the disconnect between the statement above and the
      argument that we made for the stability of the pendulum.  In the pendulum
      example, using the mechanical energy resulted in a Lyapunov function that
      was only negative semi-definite, but we eventually argued that the fixed
      points were asymptotically stable.  That took a little extra work,
      involving an argument about the fact that the fixed points were the only
      place that the system could stay with $\dot{E}=0$; every other state with
      $\dot{E}=0$ was only transient. We can formalize this idea for the more
      general Lyapunov function statements--it is known as LaSalle's
      Theorem.</p>

      <div data-type="theorem"><h1>LaSalle's Theorem</h1>

        <p>Given a system $\dot{\bx} = f(\bx)$ with $f$ continuous. If we can
        produce a scalar function $V(\bx)$ with continuous derivatives for which
        over an open-subset ${\cal B}\in \mathbf{R}^n$ we have $$V(\bx) \succ
        0,\quad \dot{V}(\bx) \preceq 0,$$ and $V(\bx)\rightarrow \infty$ as
        $||\bx||\rightarrow \infty$, then $\bx$ will converge to the largest
        <i>invariant set</i> where $\dot{V}(\bx) = 0$.</p>

      </div>

      <p>To be clear, an <em>invariant set</em>, ${\cal G}$, of the dynamical
      system is a set for which $\bx(0)\in{\cal G} \Rightarrow \forall t>0,
      \bx(t) \in {\cal G}$.  In other words, once you enter the set you never
      leave.  The "largest invariant set" need not be connected; in fact for the
      pendulum example each fixed point is an invariant set, so the largest
      invariant set is the <em>union</em> of all the fixed points of the
      system.</p>

      <p>Finding a Lyapunov function which $\dot{V} \prec 0$ is more difficult
      than finding one that has $\dot{V} \preceq 0$.  LaSalle's theorem gives us
      the ability to make a statement about <i>asymptotic</i> stability even in
      this case.  In the pendulum example, every state with $\dot\theta=0$ had
      $\dot{E}=0$, but only the fixed points are in the largest invariant
      set.</p>

    </section> <!-- lasalle -->

    <section data-type="sect2"><h1>Relationship to the Hamilton-Jacobi-Bellman
    equations</h1>

      <p> At this point, you might be wondering if there is any relationship
      between Lyapunov functions and the cost-to-go functions that we discussed
      in the context of dynamic programming.  After all, the cost-to-go
      functions also captured a great deal about the long-term dynamics of the
      system in a scalar function.  We can see the connection if we re-examine
      the HJB equation \[ 0 = \min_\bu \left[ g(\bx,\bu) +
      \pd{J^*}{\bx}f(\bx,\bu). \right] \]Let's imagine that we can solve for the
      optimizing $\bu^*(\bx)$, then  we are left with $ 0 = g(\bx,\bu^*) +
      \pd{J^*}{\bx}f(\bx,\bu^*) $ or simply \[ \dot{J}^*(\bx)  = -g(\bx,\bu^*)
      \qquad \text{vs} \qquad \dot{V}(\bx) \preceq 0. \]  In other words, in
      optimal control we must find a cost-to-go function which matches this
      gradient for every $\bx$; that's very difficult and involves solving a
      potentially high-dimensional partial differential equation.  By contrast,
      Lyapunov analysis is asking for much less - any function which is going
      downhill (at any rate) for all states.  This can be much easier, for
      theoretical work, but also for our numerical algorithms.  Also note that
      if we do manage to find the optimal cost-to-go, $J^*(\bx)$, then it can
      also serve as a Lyapunov function so long as $g(\bx,\bu^*(\bx)) \succeq
      0$.</p>

    </section> <!-- relationship to HJB -->

  </section> <!-- Lyapunov definitions -->

  <section data-type="sect1"><h1>Lyapunov analysis with convex optimization</h1>

    <p>One of the primary limitations in Lyapunov analysis as I have presented
    it so far is that it is potentially very difficult to come up with suitable
    Lyapunov function candidates for interesting systems, especially for
    underactuated systems. ("Underactuated" is almost synonymous with
    "interesting" in my vocabulary.)  Even if somebody were to give me a
    Lyapunov candidate for a general nonlinear system, the Lyapunov conditions
    can be difficult to check -- for instance, how would I check that $\dot{V}$
    is strictly negative for all $\bx$ except the origin if $\dot{V}$ is some
    arbitrarily complicated nonlinear function over a vector $\bx$?</p>

    <p>In this section, we'll look at some computational approaches to verifying
    the Lyapunov conditions, and even to searching for the description of the
    Lyapunov functions themselves.</p>

    <p>If you're imagining numerical algorithms to check the Lyapunov conditions
    for complicated Lyapunov functions and complicated dynamics, the first
    thought is probably that we can evaluate $V$ and $\dot{V}$ at a large number
    of sample points and check whether $V$ is positive and $\dot{V}$ is
    negative.  <a
    href="https://github.com/RobotLocomotion/drake/blob/master/systems/analysis/test/lyapunov_test.cc">This
    does work</a>, and could potentially be combined with some smoothness or
    regularity assumptions to generalize beyond the sample points. <todo>Add
    python bindings for the pendulum energy lp lyapunov example.</todo> But in
    many cases we will be able to do better -- providing optimization algorithms
    that will rigorously check these conditions <em>for all $\bx$</em> without
    dense sampling; these will also give us additional leverage in formulating
    the search for Lyapunov functions.  </p>

    <section data-type="sect2"><h1>Lyapunov analysis for linear systems</h1>

      <p>Let's take a moment to see how things play out for linear systems.</p>

      <div data-type="theorem"><h1>Lyapunov analysis for stable linear systems</h1>

        <p>Imagine you have a linear system, $\dot\bx = {\bf A}\bx$, and can
        find a Lyapunov function $$V(\bx) = \bx^T {\bf P} \bx, \quad {\bf P} =
        {\bf P^T} \succ 0,$$ which also satisfies $$\dot{V}(\bx) = \bx^T {\bf
        PA} \bx + \bx^T {\bf A}^T {\bf P}\bx \prec 0.$$ Then the origin is
        globally asymptotically stable.</p>

      </div>

      <p>Note that the radially-unbounded condition is satisfied by ${\bf P}
      \succ 0$, and that the derivative condition is equivalent to the matrix
      condition $${\bf PA} +  {\bf A}^T {\bf P} \prec 0.$$</p>

      <p>For stable linear systems the existence of a quadratic Lyapunov
      function is actually a necessary (as well as sufficient) condition.
      Furthermore, a Lyapunov function can always be found by finding the
      positive-definite solution to the matrix Lyapunov equation
      \begin{equation}{\bf PA} + {\bf A}^T{\bf P} = - {\bf
      Q},\label{eq:algebraic_lyapunov} \end{equation} for any ${\bf Q}={\bf
      Q}^T\succ 0$.</p>

      <todo> add an example here. double integrator? re-analyze the LQR output?
      </todo>

      <p>This is a very powerful result - for nonlinear systems it will be
      potentially difficult to find a Lyapunov function, but for linear systems
      it is straight-forward. In fact, this result is often used to propose
      candidates for non-linear systems, e.g., by linearizing the equations and
      solving a local linear Lyapunov function which should be valid in the
      vicinity of a fixed point.</p>

    </section> <!-- lyapunov for linear -->

    <section data-type="sect2"><h1>Lyapunov analysis as a semi-definite program
    (SDP)</h1>

      <p>Lyapunov analysis for linear systems has an extremely important
      connection to convex optimization. In particular, we could have also
      formulated the Lyapunov conditions for linear systems above using
      <em>semi-definite programming</em> (SDP). Semidefinite programming is a
      subset of convex optimization -- an extremely important class of problems
      for which we can produce efficient algorithms that are guaranteed find the
      global optima solution (up to a numerical tolerance and barring any
      numerical difficulties).</p>

      <p>If you don't know much about convex optimization or want a quick
      refresher, please take a few minutes to read the optimization
      preliminaries in the appendix.  The main requirement for this section is
      to appreciate that it is possible to formulate efficient optimization
      problems where the constraints include specifying that one or more
      matrices are positive semi-definite (PSD).  These matrices must be formed
      from a linear combination of the decision variables.  For a trivial
      example, the optimization $$\min_a a,\quad \subjto \begin{bmatrix} a & 0
      \\ 0 & 1 \end{bmatrix} \succeq 0,$$ returns $a = 0$ (up to numerical
      tolerances).</p>

      <p>The value in this is immediate for linear systems.  For example, we can
      formulate the search for a Lyapunov function for the linear system
      $\dot\bx = {\bf A} \bx$ by using the parameters ${\bf p}$ to populate a
      symmetric matrix ${\bf P}$ and then write the SDP: \[ \find_{\bf p} \quad
      \subjto \quad {\bf P} \succeq 0, \quad {\bf PA} + {\bf A}^T {\bf P}
      \preceq 0.\]  Note that you would probably never use that particular
      formulation, since there specialized algorithms for solving the simple
      Lyapunov equation which are more efficient and more numerically stable.
      But the SDP formulation does provide something new -- we can now easily
      formulate the search for a <i>"common Lyapunov function"</i> for uncertain
      linear systems.</p>

      <div data-type="example" class="drake"><h1>Common Lyapunov analysis for
      linear systems</h1>

        <p>Suppose you have a system governed by the equations $\dot\bx = {\bf
        A}\bx$, where the matrix ${\bf A}$ is unknown, but its uncertain
        elements can be bounded.  There are a number of ways to write down this
        uncertainty set; let us choose to write this by describing ${\bf A}$ as
        the affine combination of a number of known matrices, $${\bf A} =
        \sum_{i} \beta_i {\bf A}_i, \quad \sum_i \beta_i = 1, \quad \forall i,
        \beta_i > 0.$$  This is just one way to specify the uncertainty;
        geometrically it is describing a polygon of uncertain parameters (in the
        space of elements of ${\bf A}$ with each ${\bf A}_i$ as one of the
        vertices in the polygon.</p>

        <figure> <img width="50%"
        data-src="figures/affine_combination_of_matrices.svg"/> </figure>

        <p>Now we can formulate the search for a common Lyapunov function using
        \[ \find_{\bf p} \quad  \subjto \quad {\bf P} \succeq 0,  \quad
        \forall_i, {\bf PA}_i + {\bf A}_i^T {\bf P} \preceq 0.\]  The solver
        will then return a matrix ${\bf P}$ which satisfies all of the
        constraints, or return saying "problem is infeasible".  It can easily be
        verified that if ${\bf P}$ satisfies the Lyapunov condition at all of
        the vertices, then it satisfies the condition for every ${\bf A}$ in the
        set:  \[ {\bf P}(\sum_i \beta_i {\bf A}_i) + (\sum_i \beta_i {\bf
        A}_i)^T {\bf P} = \sum_i \beta_i ({\bf P A}_i + {\bf A}_i^T {\bf P})
        \preceq 0, \] since $\forall i$, $\beta_i > 0$.  Note that, unlike the
        simple Lyapunov equation for a known linear system, this condition being
        satisfied is a sufficient but not a necessary condition -- it is
        possible that the set of uncertain matrices ${\bf A}$ is robustly
        stable, but that this stability cannot be demonstrated with a common
        quadratic Lyapunov function.</p>

        <p> You can try this example for yourself in <drake></drake>.</p>

        <pysrc>lyapunov/linear_systems_common_lyapunov.py</pysrc>

        <p>As always, make sure that you open up the code and take a look.</p>

      </div>

      <p>This example is very important because it establishes a connection
      between Lyapunov functions and (convex) optimization.  But so far we've
      only demonstrated this connection for linear systems where the PSD
      matrices provide a magical recipe for establishing the positivity of the
      (quadratic) functions for all $\bx$.  Is there any hope of extending this
      type of analysis to more general nonlinear systems?  Surprisingly, it
      turns out that there is.</p>

    </section>

    <section data-type="sect2"><h1>Sums-of-squares optimization</h1>

      <p>It turns out that in the same way that we can use SDP to search over
      the positive quadratic equations, we can generalize this to search over
      the positive polynomial equations.  To be clear, for quadratic equations
      we have \[ {\bf P}\succeq 0 \quad \Rightarrow \quad \bx^T {\bf P} \bx \ge
      0, \quad \forall \bx. \]  It turns out that we can generalize this to \[
      {\bf P}\succeq 0 \quad \Rightarrow \quad {\bf m}^T(\bx) {\bf P} {\bf
      m}(\bx) \ge 0, \quad \forall \bx, \] where ${\bf m}(\bx)$ is a vector of
      polynomial equations, typically chosen as a vector of <em>monomials</em>
      (polynomials with only one term).  The set of positive polynomials
      parameterized in this way is exactly the set of polynomials that can be
      written as a <em>sum of squares</em><elib>Parrilo00</elib>.  While it is
      known that not all positive polynomials can be written in this form, much
      is known about the gap (and papers have been written about trying to find
      polynomials which are uniformly positive but not sums of squares).  For
      our purposes this gap is very small; we should remember that it exists but
      not worry about it too much for now.</p>

      <p>Even better, there is quite a bit that is known about how to choose the
      terms in ${\bf m}(\bx)$.  For example, if you give me the polynomial \[
      p(x) = 2 - 4x + 5x^2 \] and ask if it is positive for all real $x$, I can
      convince you that it is by producing the sums-of-squares factorization \[
      p(x) = 1 + (1-2x)^2 + x^2, \] and I know that I can formulate the problem
      without needing any monomials of degree greater than 1 (the square-root of
      the degree of $p$) in my monomial vector.  In practice, what this means
      for us is that people have authored optimization front-ends which take a
      high-level specification with constraints on the positivity of polynomials
      and they automatically generate the SDP problem for you, without having to
      think about what terms should appear in ${\bf m}(\bx)$ (c.f.
      <elib>Prajna04</elib>). This class of optimization problems is called
      Sums-of-Squares (SOS) optimization.</p>

      <p>We will write optimizations using sums-of-squares constraints as \[
      p(\bx) \sos \] as shorthand for the constraint that $p(\bx) \ge 0$ for all
      $\bx$, as demonstrated by finding a sums-of-squares decomposition.</p>

    </section>

    <section data-type="sect2"><h1>Lyapunov analysis for polynomial systems</h1>

      <p>Now we can see that it may be possible to generalize the optimization
      approach using SDP to search for Lyapunov functions for linear systems to
      searching for Lyapunov functions for at least the polynomial systems:
      $\dot\bx = f(\bx),$ where $f$ is a vector-valued polynomial function.  If
      we parameterize a <em>fixed-degree</em> Lyapunov candidate as a polynomial
      with unknown coefficients, e.g., \[ V_\alpha(\bx) = \alpha_0 + \alpha_1
      x_1 + \alpha_2 x_2 + \alpha_3 x_1x_2 + \alpha_4 x_1^2 + ..., \] then the
      time-derivative of $V$ is also a polynomial, and I can formulate the
      optimization: \begin{align*} \find_\alpha, \quad \subjto \quad&
      V_\alpha(\bx) \sos \\ & -\dot{V}_\alpha(\bx) = -\pd{V_\alpha}{\bx} f(\bx)
      \sos. \end{align*}  Because this is a convex optimization, the solver will
      return a solution if one exists.</p>

      <div data-type="example" class="drake"><h1>Global stability of the simple
      pendulum via SOS</h1>

        <p> We opened this chapter using our intuition about energy to discuss
        stability on the simple pendulum. Now we'll replace that intuition with
        convex optimization (because it will also work for more difficult
        systems where our intuition fails).</p>

        <p>But wait!  The equations of motion of the simple pendulum are not
        polynomial, are they (they have a $\sin(\theta)$ in them)?  Actually
        nearly all rigid-body systems can be written as polynomials given a
        change of coordinates.  Indeed, the rigid-body-assumption is simply an
        assumption that points on the same body stay a constant distance apart,
        and Euclidean distance is certainly a polynomial.  For a more thorough
        discussion see, for instance, <elib>Wampler11,Sommese05</elib>.</p>

        <p>For the purposes of this example, let's change coordinates from
        $[\theta,\dot\theta]^T$ to $\bx = [s,c,\dot\theta]^t$, where $s \equiv
        \sin\theta$ and $c \equiv \cos\theta$.  Then we can write the pendulum
        dynamics as $$\dot\bx = \begin{bmatrix} c \dot\theta \\ -s \dot\theta \\
        -\frac{1}{m l^2} \left( b \dot\theta + mgls \right) \end{bmatrix}.$$
        </p>

        <p>Now let's parameterize a Lyapunov candidate $V(s,c,\dot\theta)$ as
        the polynomial with unknown coefficients which contains all monomials up
        to degree 2:  $$V = \alpha_0 + \alpha_1 s + \alpha_2 c + ... \alpha_{9}
        s^2 + \alpha_{10} sc + \alpha_{11} s\dot\theta.$$ Now we'll formulate
        the feasibility problem: \[ \find_{\bf \alpha} \quad  \subjto \quad V
        \sos, \quad -\dot{V} \sos.\] In fact, this is asking too much -- really
        $\dot{V}$ only needs to be negative when $s^2+c^2=1$.  We can accomplish
        this with the so-called <i>S-procedure</i>, which we will discuss more
        below, and instead write \[ \find_{{\bf \alpha},\lambda} \quad  \subjto
        \quad V \sos, \quad -\dot{V} -\lambda(\bx)(s^2+c^2-1) \sos.\] (Here
        $\lambda(\bx)$ is another polynomial with free coefficients which the
        optimization can use to make terms arbitrarily more positive when
        $s^2+c^2 \neq 1$.) Finally, for style points, in the code example in
        <drake></drake> we ask for exponential stability:</p>

        <todo>Add python example</todo>
        <p><b>Python version coming soon (try src/lypunov/cubic_polyonomial.py
        if your eager).</b></p>

        <p>As always, make sure that you open up the code and take a look.  The
        result is a Lyapunov function that looks familiar (visualized as a
        countour plot here):</p>

        <div  style="text-align:center"><img data-src="figures/global_pend_V.svg" width="400px"/></div>

        <p>Aha!  Not only does the optimization finds us coefficients for the
        Lyapunov function which satisfy our Lyapunov conditions, but the result
        looks a lot like mechanical energy.  In fact, the result is a little
        better than energy... there are some small extra terms added which prove
        exponential stability without having to invoke LaSalle's Theorem.</p>

      </div>

      <p>It is important to remember that there are a handful of gaps which make
      the existence of this solution a sufficient condition (for proving that
      every sub-level set of $V$ is an invariant set of $f$) instead of a
      necessary one.  First, there is no guarantee that a stable polynomial
      system can be verified using a polynomial Lyapunov function (of any
      degree, and in fact there are known counter-examples <todo>add ref</todo>)
      and here we are only searching over a fixed-degree polynomial.  Second,
      even if a polynomial Lyapunov function does exist, there is a gap between
      the SOS polynomials and the positive polynomials.</p>

      <p>Despite these caveats, I have found this formulation to be surprisingly
      effective in practice.  Intuitively, I think that this is because there is
      relatively a lot of flexibility in the Lyapunov conditions -- if you can
      find one function which is a Lyapunov function for the system, then there
      are also many "nearby" functions which will satisfy the same
      constraints.</p>

    </section>

  </section> <!-- computing Lyapunov -->

  <section data-type="sect1"><h1>Lyapunov functions for estimating regions of
  attraction</h1>

    <p>There is another very important connection between Lyapunov functions and
    the concept of an invariant set: <em>any sub-level set of a Lyapunov
    function is also an invariant set</em>.  This gives us the ability to use
    sub-level sets of a Lyapunov function as approximations of the region of
    attraction for nonlinear systems.</p>

    <div data-type="theorem"><h1>Lyapunov invariant set and region of attraction
    theorem</h1>

      <p>Given a system $\dot{\bx} = f(\bx)$ with $f$ continuous, if we can find
      a scalar function $V(\bx) \succ 0$ and a sub-level set $${\cal G}: \{ \bx
      | V(\bx) < \rho \}$$ on which $$\forall \bx \in {\cal G}, \dot{V}(\bx)
      \preceq 0,$$ then ${\cal G}$ is an invariant set.  By LaSalle, $\bx$ will
      converge to the largest invariant subset of ${\cal G}$ on which
      $\dot{V}=0$.<p>

      <p>Furthermore, if $\dot{V}(\bx) \prec 0$ in ${\cal G}$, then the origin
      is locally asymptotically stable and the set ${\cal G}$ is inside the
      region of attraction of this fixed point.  Alternatively, if $\dot{V}(\bx)
      \preceq 0$ in ${\cal G}$ and $\bx = 0$ is the only invariant subset of
      ${\cal G}$ where $\dot{V}=0$, then the origin is asymptotically stable and
      the set ${\cal G}$ is inside the region of attraction of this fixed point.
      </p>

    </div>

    <div data-type="example"><h1>Region of attraction for a one-dimensional
    system</h1>

      <p> Consider the first-order, one-dimensional system $\dot{x} = -x + x^3.$
      We can quickly understand this system using our tools for graphical
      analysis.</p> <figure> <img width="80%"
      data-src="figures/cubicPolynomialExample.svg"/> </figure> <p>In the
      vicinity of the origin, $\dot{x}$ looks like $-x$, and as we move away it
      looks increasingly like $x^3$.  There is a stable fixed point at the
      origin and unstable fixed points at $\pm 1$.  In fact, we can deduce
      visually that the region of attraction to the stable fixed point at the
      origin is $\bx \in (-1,1)$.  Let's see if we can demonstrate this with a
      Lyapunov argument.</p>

      <p>First, let us linearize the dynamics about the origin and use the
      Lyapunov equation for linear systems to find a candidate $V(\bx)$.  Since
      the linearization is $\dot{x} = -x$, if we take ${\bf Q}=1$, then we find
      ${\bf P}=\frac{1}{2}$ is the positive definite solution to the algebraic
      Lyapunov equation (\ref{eq:algebraic_lyapunov}).  Proceeding with $$V(\bx)
      = \frac{1}{2} x^2,$$ we have $$\dot{V} = x (-x + x^3) = -x^2 + x^4.$$
      This function is zero at the origin, negative for $|x| < 1$, and positive
      for $|x| > 1$.  Therefore we can conclude that the sub-level set $V <
      \frac{1}{2}$ is invariant and the set $x \in (-1,1)$ is inside the region
      of attraction of the nonlinear system.  In fact, this estimate is
      tight.</p>

    </div>

    <section data-type="sect2"><h1>Robustness analysis using "common Lyapunov functions"</h1>

      <p>While we will defer most discussions on robustness analysis until later
      in the notes, there is a simple and powerful idea that can be presented
      now: the idea of a <em>common Lyapunov function</em>.  Imagine that you
      have a model of a dynamical system but that you are uncertain about some
      of the parameters.  For example, you have a model of a quadrotor, and are
      fairly confident about the mass and lengths (both of which are easy to
      measure), but are not confident about the moment of inertia.  One approach
      to robustness analysis is to define a bounded uncertainty, which could
      take the form of $$\dot{\bx} = f_\alpha(\bx), \quad \alpha_{min} \le
      \alpha \le \alpha_{max},$$ with $\alpha$ a vector of uncertain parameters
      in your model.  Richer specifications of the uncertainty bounds are also
      possible, but this will serve for our examples.</p>

      <p>In standard Lyapunov analysis, we are searching for a function that
      goes downhill for all $\bx$ to make statements about the long-term
      dynamics of the system.  In common Lyapunov analysis, we can make many
      similar statements about the long-term dynamics of an uncertain system if
      we can find a single Lyapunov function that goes downhill <em>for all
      possible values of $\alpha$</em>.  If we can find such a function, then we
      can use it to make statements with all of the variations we've discussed
      (local, regional, or global; in the sense of Lyapunov, asymptotic, or
      exponential).</p>

      <div data-type="example"><h1>A one-dimensional system with gain uncertainty</h1>

        <p>Let's consider the same one-dimensional example used above, but add
        an uncertain parameter into the dynamics.  In particular, consider the
        system: $$\dot{x} = -x + \alpha x^3, \quad \frac{3}{4} < \alpha <
        \frac{3}{2}.$$ Plotting the graph of the one-dimensional dynamics for a
        few values of $\alpha$, we can see that the fixed point at the origin is
        still stable, but the <em>robust region of attraction</em> to this fixed
        point (shaded in blue below) is smaller than the region of attraction
        for the system with $\alpha=1$.</p>

        <figure> <img width="80%"
        data-src="figures/cubicPolynomialExample_gain_uncertainty.svg"/>
        </figure>

        <p>Taking the same Lyapunov candidate as above, $V = \frac{1}{2} x^2$,
        we have $$\dot{V} = -x^2 + \alpha x^4.$$  This function is zero at the
        origin, and negative for all $\alpha$ whenever $x^2 > \alpha x^4$, or
        $$|x| < \frac{1}{\sqrt{\alpha_{max}}} = \sqrt{\frac{2}{3}}.$$ Therefore,
        we can conclude that $|x| < \sqrt{\frac{2}{3}}$ is inside the robust
        region of attraction of the uncertain system.</p>

      </div>

      <p>Not all forms of uncertainty are as simple to deal with as the gain
      uncertainty in that example.  For many forms of uncertainty, we might not
      even know the location of the fixed points of the uncertain system.  In
      this case, we can often still use common Lyapunov functions to give some
      guarantees about the system, such as guarantees of <em>robust set
      invariance</em>.  For instance, if you have uncertain parameters on a
      quadrotor model, you might be ok with the quadrotor stabilizing to a pitch
      of $0.01$ radians, but you would like to guarantee that it definitely does
      not flip over and crash into the ground.</p>

      <div data-type="example"><h1>A one-dimensional system with additive
      uncertainty</h1>

        <p>Now consider the system: $$\dot{x} = -x + x^3 + \alpha, \quad
        -\frac{1}{4} < \alpha < \frac{1}{4}.$$  Plotting the graph of the
        one-dimensional dynamics for a few values of $\alpha$, this time we can
        see that the fixed point is not necessarily at the origin;  the location
        of the fixed point moves depending on the value of $\alpha$.  But we
        should be able to guarantee that the uncertain system will stay near the
        origin if it starts near the origin, using an invariant set
        argument.</p>

        <figure>
          <img width="80%" data-src="figures/cubicPolynomialExample_additive_uncertainty.svg"/>
        </figure>

        <p>Taking the same Lyapunov candidate as above, $V = \frac{1}{2} x^2$,
        we have $$\dot{V} = -x^2 + x^4 + \alpha x.$$ This Lyapunov function
        allows us to easily verify, for instance, that $V \le \frac{1}{3}$ is a
        <em>robust invariant set</em>, because whenever $V = \frac{1}{3}$, we
        have $$\forall \alpha \in [\alpha_{min},\alpha_{max}],\quad
        \dot{V}(x,\alpha) < 0.$$  Therefore $V$ can never start at less than
        one-third and cross over to greater than one-third.  To see this, see
        that $$ V=\frac{1}{3} \quad \Rightarrow \quad x = \pm \sqrt{\frac{2}{3}}
        \quad \Rightarrow \quad \dot{V} = -\frac{2}{9} \pm \alpha
        \sqrt{\frac{2}{3}} < 0, \forall \alpha \in
        \left[-\frac{1}{4},\frac{1}{4} \right]. $$  Note that not all sub-level
        sets of this invariant set are invariant.  For instance $V <
        \frac{1}{32}$ does not satisfy this condition, and by visual inspection
        we can see that it is in fact not robustly invariant.</p>

      </div>

    <todo>robust quadrotor example</todo>

    </section> <!-- end common lyap -->

    <section data-type="sect2"><h1>Region of attraction estimation for
    polynomial systems</h1>

      <p>Now we have arrived at the tool that I believe can be a work-horse for
      many serious robotics applications.  Most of our robots are not actually
      globally stable (that's not because they are robots -- if you push me hard
      enough, I will fall down, too), which means that understanding the regions
      where a particular controller can be guaranteed to work can be of critical
      importance.</p>

      <p>Sums-of-squares optimization effectively gives us an oracle which we
      can ask: is this polynomial positive for all $\bx$?  To use this for
      regional analysis, we have to figure out how to modify our questions to
      the oracle so that the oracle will say "yes" or "no" when we ask if a
      function is positive over a certain region which is a subset of $\Re^n$.
      That trick is called the S-procedure.  It is closely related to the
      Lagrange multipliers from constrained optimization, and has deep
      connections to "Positivstellensatz" from algebraic geometry.</p>

      <section data-type="sect3"><h1>The S-procedure</h1>

        <p> Consider a scalar polynomial, $p(\bx)$, and a semi-algebraic set
        $g(\bx) \preceq 0$, where $g$ is a vector of polynomials.  If I can find
        a polynomial "multiplier", $\lambda(\bx)$, such that \[ p(\bx) +
        \lambda^T(\bx) g(\bx) \sos, \quad \text{and} \quad \lambda(\bx) \sos, \]
        then this is sufficient to demonstrate that $$p(\bx)\ge 0 \quad \forall
        \bx \in \{ g(\bx) \le 0 \}.$$  To convince yourself, observe that when
        $g(\bx) \le 0$, it is only harder to be positive, but when $g(\bx) > 0$,
        it is possible for the combined function to be SOS even if $p(\bx)$ is
        negative.</p>

      </section>

      <section data-type="sect3"><h1>Basic region of attraction formulation</h1>

        <div data-type="example" class="drake"><h1>Region of attraction for the one-dimensional cubic system</h1>

          <p>Let's return to our example from above: \[ \dot{x} = -x + x^3 \]
          and try to use SOS optimization to demonstrate that the region of
          attraction of the fixed point at the origin is $x \in (-1,1)$, using
          the Lyapunov candidate $V = x^2.$ </p>

          <p>First, define the multiplier polynomial, \[ \lambda(x) = c_0 + c_1
          x + c_2 x^2 + c_3 x^3 + c_4 x^4. \]  Then define the optimization
          \begin{align*} \find_{\bf c} \quad & \\  \subjto \quad& - \dot{V}(x) -
          \lambda(x) (1-V(x)) \sos \\ & \lambda(x) \sos \end{align*} </p>

          <p>You can try this example for yourself in <drake></drake>.</p>

          <pysrc>lyapunov/cubic_polynomial.py</pysrc>

        </div>

        <p>While the example above only verifies that the one-sub-level set of
        the pre-specified Lyapunov candidate is negative (certifying the ROA
        that we already understood), we can generalize the optimization to allow
        us to search for the largest sub-level set (with the objective using a
        convex approximation for volume).  We can even search for the
        coefficients of the Lyapunov function using an iteration of convex
        optimizations.  There are a number of variations and nuances in the
        various formulations, and some basic rescaling tricks that can help make
        the numerics of the problem better for the solvers.</p>


        <todo> make this true for pydrake:

        In <drake></drake>, we have packaged all of these up into a
        <code>regionOfAttraction</code> method that is defined for objects of
        type <code>PolynomialSystem</code>. This makes it as simple as, for
        instance:

        <div data-type="example" class="drake"><h1>Searching for the region of
          attraction</h1> <p> <pre><code class="matlab"
          testfile="testCubicPolynomialRoa">
          cd(fullfile(getDrakePath,'examples'));

          % create a new CubicPolynomialExample object
          p = CubicPolynomialExample();

          % compute region of attraction
          % the levelset V<1 is the region of attraction
          V=regionOfAttraction(p,Point(p.getStateFrame,0));

          % display the polynomial representation of V that results
          display(V.getPoly);
          </code></pre>

          which yields the console output:
          <pre><code>
          [  (1)*x1^2  ]
          </code>
          </pre>
          The algorithm successfully found that $x \in (-1,1)$ is the region of attraction.
        </div>

        <p>Remember that although we have tried to make it convenient to call
        these functions, they are not a black box.  I highly recommend opening
        up the <code>regionOfAttraction</code> method and understanding how it
        works.</p>

        <div data-type="example" class="drake"><h1>Region of attraction for the time-reversed Van der Pol oscillator</h1>

          <pre><code class="matlab" testfile="testVanDerPol">
           cd(fullfile(getDrakePath,'examples'));
           VanDerPol.run();
          </code></pre>

        </div>
        </todo>

      </section>

      <section data-type="sect3"><h1>Seeding the optimization with linear analysis</h1>

        <p>LQR gives the cost-to-go which can be used as the Lyapunov candidate.
        Otherwise, use a Lyapunov equation.  You may not even need to search for
        a better Lyapunov function, but rather just ask the question: what is
        the largest region of attraction that can be demonstrated for the
        nonlinear system using the Lyapunov function from linear analysis?</p>

      </section>

    </section> <!-- ROA for polynomial -->

    <section data-type="sect2"><h1>Rigid-body dynamics are polynomial</h1>

      <p>Coming soon... For a nice explanation of how rigid-body kinematics are
      polynomial, see <elib>Wampler11</elib>.  Looking at the Lagrangian, you
      can see that if the kinematics are polynomial, then the dynamics will be,
      too.</p>

      <p>In practice, you can also Taylor approximate any smooth nonlinear
      function using polynomials.  This can be an effective strategy in
      practice, because you can limit the degrees of the polynomial, and because
      in many cases it is possible to provide conservative bounds on the errors
      due to the approximation.</p>

    </section>

    <todo> implicit dynamics formulations.  convex ROA w/ outer approx.  control
    design.  </todo>

  </section> <!-- end ROA -->

</section> <!-- ***************  end of Lyapunov chapter **************   -->

<section data-type="chapter" class="chapter" id="trajopt"><h1>Trajectory
  Optimization</h1>

  <p>I've argued that optimal control is a powerful framework for specifying
  complex behaviors with simple objective functions, letting the dynamics and
  constraints on the system shape the resulting feedback controller (and vice
  versa!). But the computational tools that we've provided so far have been
  limited in some important ways.  The numerical approaches to dynamic
  programming which involve putting a mesh over the state space do not scale
  well to systems with state dimension more than four or five.  Linearization
  around a nominal operating point (or trajectory) allowed us to solve for
  locally optimal control policies (e.g. using LQR) for even very
  high-dimensional systems, but the effectiveness of the resulting controllers
  is limited to the region of state space where the linearization is a good
  approximation of the nonlinear dynamics.  The computational tools for Lyapunov
  analysis from the last chapter can provide, among other things, an effective
  way to compute estimates of those regions.  But we have not yet provided any
  real computational tools for approximate optimal control that work for
  high-dimensional systems beyond the linearization around a goal. That is
  precisely the goal for this chapter.</p>

  <p>The big change that is going to allow us to scale to high-dimensional
  systems is that we are going to give up the goal of solving for the optimal
  feedback controller for the entire state space, and instead attempt to find an
  optimal control solution that is valid from only a single initial condition.
  Instead of representing this as a feedback control function, we can represent
  this solution as a <em>trajectory</em>, $\bx(\cdot), \bu(\cdot)$, typically
  defined over a finite interval.  In our graph-search dynamic programming
  algorithms, we discretized the dynamics of the system on a mesh spread across
  the state space.  This does not scale to high-dimensional systems, and it was
  difficult to bound the errors due to the discretization.  If we instead
  restrict ourselves to optimizing only a single initial condition, then a
  different discretization scheme emerges: we can discretize the state and input
  trajectories <em>over time</em>.</p>

  <section data-type="sect1"><h1>Problem Formulation</h1>

    <p>Given an initial condition, $\bx_0$, and an input trajectory $u(t)$
    defined over a finite interval, $t\in[t_0,t_f]$,   we can compute the
    long-term (finite-horizon) cost of executing that trajectory   using the
    standard additive-cost optimal control objective, \[ J_{\bu(\cdot)}(\bx_0) =
    \int_{t_0}^{t_f} g(\bx(t),\bu(t)) dt. \]  We will write the trajectory
    optimization problem as \begin{align*} \minimize_{u(\cdot)} \quad &
    \int_{t_0}^{t_f} g(\bx(t),\bu(t)) dt \\ \subjto \quad & \forall t,
    \dot{\bx}(t) = f(\bx(t),\bu(t)), \\ & \bx(t_0) = \bx_0. \\ \end{align*} Some
    trajectory optimization problems may also include additional constraints,
    such as collision avoidance ($\bx$ can not cause the robot to be inside an
    obstacle) or input limits (e.g. $\bu_{min} \le \bu \le \bu_{max}$ ), which
    can be defined for all time or some subset of the trajectory.</p>

    <p> As written, the optimization above is an optimization over continuous
    trajectories.  In order to formulate this as a numerical optimization, we
    must parameterize it with a finite set of numbers.  Perhaps not
    surprisingly, there are many different ways to write down this
    parameterization, with a variety of different properties in terms of speed,
    robustness, and accuracy of the results. We will outline just a few of the
    most popular below.  I would recommend <elib>Betts98+Betts01</elib> for
    additional details.</p>

  </section>

  <section data-type="sect1"><h1>Computational Tools for Nonlinear
  Optimization</h1>

    <p>Before we dive in, we need to take a moment to understand the
    optimization tools that we will be using.  In the graph-search dynamic
    programming algorithm, we were magically able to provide an iterative
    algorithm that was known to converge to the optimal cost-to-go function.
    With LQR we were able to reduce the problem to a matrix Riccati equation,
    for which we have scalable numerical methods to solve.  In the Lyapunov
    analysis chapter, we were able to formulate a very specific kind of
    optimization problem--a semi-definite program (or SDP)--which is a subset of
    convex optimization, and relied on custom solvers like Gurobi or Mosek to
    solve the problems for us. Convex optimization is a hugely important subset
    of nonlinear optimization, in which we can guarantee that the optimization
    has no "local minima". In this chapter we won't be so lucky; most of the
    optimizations that we formulate may have local minima and the solution
    techniques will at best only guarantee that they give a locally optimal
    solution.</p>

    <p>The generic formulation of a nonlinear optimization problem is     \[
    \minimize_z c(z) \quad \subjto \quad \bphi(z) \le 0, \]     where $z$ is a
    vector of <em>decision variables</em>, $c$ is a scalar <em>objective
    function</em> and $\phi$ is a vector     of <em>constraints</em>.  Note
    that, although we write $\phi \le 0$, this formulation     captures
    positivity constraints on the decision variables (simply multiply the
    constraint by $-1$) and equality constraints (simply list both $\phi\le0$
    and $-\phi\le0$) as well. </p>

    <p>The picture that you should have in your head is a nonlinear, potentially
    non-convex objective function defined over (multi-dimensional) $z$, with a
    subset of possible $z$ values satisfying the constraints.</p>

    <figure> <img width="80%"
    data-src="figures/nonlinear_optimization_w_minima.svg"/>
    <figcaption>One-dimensional cartoon of a nonlinear optimization problem. The
    red dots represent local minima.  The blue dot represents the optimal
    solution.</figcaption> </figure>

    <p>Note that minima can be the result of the objective function having zero
    derivative <em>or</em> due to a sloped objective up against a
    constraint.</p>

    <p>Numerical methods for solving these optimization problems require an
    initial guess, $\hat{z}$, and proceed by trying to move down the objective
    function to a minima.  Common approaches include <em>gradient descent</em>,
    in which the gradient of the objective function is computed or estimated, or
    second-order methods such as <em>sequential quadratic programming (SQP)</em>
    which attempts to make a local quadratic approximation of the objective
    function and local linear approximations of the constraints and solves a
    quadratic program on each iteration to jump directly to the minimum of the
    local approximation.</p>

    <p>While not strictly required, these algorithms can often benefit a great
    deal from having the gradients of the objective and constraints computed
    explicitly; the alternative is to obtain them from numerical
    differentiation.  Beyond pure speed considerations, I strongly prefer to
    compute the gradients explicitly because it can help avoid numerical
    accuracy issues that can creep in with finite difference methods.  The
    desire to calculate these gradients will be a major theme in the discussion
    below, and we have gone to great lengths to provide explicit gradients of
    our provided functions and automatic differentiation of user-provided
    functions in <drake></drake>.</p>

    <p>When I started out, I was of the opinion that there is nothing difficult
    about implementing gradient descent or even a second-order method, and I
    wrote all of the solvers myself.  I now realize that I was wrong.  The
    commercial solvers available for nonlinear programming are substantially
    higher performance than anything I wrote myself, with a number of tricks,
    subtleties, and parameter choices that can make a huge difference in
    practice.  Some of these solvers can exploit sparsity in the problem (e.g.,
    if the constraints operate in a sparse way on the decision variables).
    Nowadays, we make heaviest use of SNOPT <elib>Gill06</elib>, which now comes
    bundled with the binary distributions of <drake></drake>, but also <a
    href="http://drake.mit.edu/doxygen_cxx/group__solvers.html">support a large
    suite of numerical solvers</a>.  Note that while I do advocate using these
    tools, you do not need to use them as a black box.  In many cases you can
    improve the optimization performance by understanding and selecting
    non-default configuration parameters.</p>

  </section> <!-- nonlinear optimization -->

  <section data-type="sect1"><h1>Trajectory optimization as a nonlinear
  program</h1>

    <section data-type="sect2"><h1>Direct Transcription</h1>

      <p> Let us start by representing the finite-time trajectories, $\bx(t)$
      and $\bu(t)$ $\forall t\in[t_0,t_f]$, by their values at a series of
      <em>break points</em>, $t_0,t_1,t_2,t_3$, and denoting the values at those
      points (aka the <em>knot points</em>) $\bx_0,...,\bx_N$ and
      $\bu_0,...,\bu_N$, respectively.</p>

      <p>Then perhaps the simplest mapping of the trajectory optimization
      problem onto a nonlinear program is to fix the break points at even
      intervals, $dt$, and use Euler integration to write \begin{align*}
      \minimize_{\bx_1,...,\bx_N,\bu_0,...,\bu_{N-1}} & \sum_{n=0}^{N-1}
      g(\bx_n,\bu_n)dt \\ & \bx_{n+1} = \bx_n + f(\bx_n,\bu_n)dt, \quad \forall
      n\in [0,N-1]. \end{align*} Note that the decision variables here are
      $(\bx_1,...,\bx_N,\bu_0,...,\bu_{N-1})$, because $\bx_0$ is given, and
      $\bu_{N}$ does not appear in the cost nor any of the constraints.  It is
      easy to generalize this approach to add additional costs or constraints on
      $\bx$ and/or $\bu$. (Note that this formulation does not actually benefit
      from the additive cost structure, so more general cost formulations are
      also possible.)  Computing the gradients of the objective and constraints
      is essentially as simple as computing the gradients of $g()$ and $f()$.
      </p>

      <!-- <todo>re-implement this.  note that it is nonlinear if dt is a decision variable (which could make sense for the minimum-time problem).</todo>
      https://github.com/RobotLocomotion/drake/blob/last_sha_with_original_matlab/drake/examples/DoubleIntegrator.m#L125
      might actually be better to replace it with the pendulum (and fixed dt).

      <div data-type="example" class="drake"><h1>Direct Transcription for the
      Double Integrator</h1>

        <p>We have implemented an optimization class hierarchy in
        <drake></drake> which makes it easy to try out these algorithms.
        Watching the way that they perform on our simple problems is a very nice
        way to gain intuition.  Here is some simple code to solve the
        (time-discretized) minimum-time problem for the double integrator.</p>

        <pre><code class="matlab" testfile="testDoubleIntegratorDirtran">
        % note: requires Drake ver >= 0.9.7

        cd(fullfile(getDrakePath,'examples'));
        DoubleIntegrator.runDirtran;
        </code></pre>

        Make sure you take a look at the code:
        <pre><code class="matlab">
        edit('DoubleIntegrator.runDirtran')
        </code></pre>

        <p>Nothing compares to running it yourself, and poking around in the
        code.  But you can also click <a target="_blank"
        href="figures/dirtran_mintime_double_integrator.swf">here</a> to watch
        the result. I hope that you recognize the parabolic trajectory from the
        initial condition up to the switching surface, and then the second
        parabolic trajectory down to the origin.  You should also notice that
        the transition between $u=1$ and $u=-1$ is imperfect, due to the
        discretization error.  As an exercise, try increasing the number of knot
        points (the variable <code>N</code> in the code) to see if you can get a
        sharper response, like <a target="_blank"
        href="figures/dirtran_mintime_double_integrator_fine.swf">this.</a></p>

      </div>
      -->

      <p>If you take a moment to think about what the direct transcription
      algorithm is doing,   you will see that by satisfying the dynamic
      constraints, the optimization is effectively   solving the (Euler
      approximation of the) differential equation.  But instead of marching
      forward through time, it is minimizing the inconsistency at each of the
      knot points simultaneously.   While it's easy enough to generalize the
      constraints to use higher-order integration schemes,   paying attention to
      the trade-off between the number of times the constraint must be evaluated
      vs the density of the knot points in time, if the goal is to obtain smooth
      $\bx$ trajectory   solutions then another approach quickly emerges: the
      approach taken by the so-called <em>collocation methods</em>.</p>

    </section> <!-- end dirtran -->

    <section data-type="sect2"><h1>Direct Collocation</h1>

      <p> In direct collocation (c.f., <elib>Hargraves87</elib>), both the input
      trajectory and the state trajectory are   represented explicitly as
      piecewise polynomial functions.  In particular, the sweet   spot for this
      algorithm is taking $\bu(t)$ to be a first-order polynomial -- allowing it
      to   be completely defined by the values at the knot points -- and
      $\bx(t)$ to be a Hermite   cubic polynomial -- completely defined by the
      values and derivatives at the knot points.  The state derivatives at the
      knot points are given as a function of $\bx$ and $\bu$ by the plant
      dynamics, so the entire spline   is completely defined by the values of
      $\bx$ and $\bu$ at the knot points.  To add the additional constraint that
      $\bx_{n+1}$ is dynamically consistent with $\bx_{n}$ and $\bu_{n}$, we add
      an additional set of constraints that the derivative at a set of
      <em>collocation points</em> also matches the plant dynamics.  For the
      special case of cubic polynomial state trajectories and piecewise linear
      input trajectories, the derivative at the midpoint of each segment is
      particularly easy to compute, making it the natural choice.</p>

      <figure>
        <img width="80%" data-src="figures/dircol.svg">
        <figcaption>Cubic spline parameters used in the direct collocation method.</figcaption>
      </figure>

      <p>Once again, direct collocation effectively integrates the equations of
      motion by satisfying the constraints of the optimization -- this time
      producing a third-order approximation of the dynamics with effectively two
      evaluations of the plant dynamics per segment.  <elib>Hargraves87</elib>
      claims, without proof, that as the break points are brought closer
      together, the trajectory will converge to a true solution of the
      differential equation.  Once again it is very natural to add additional
      terms to the cost function or additional input/state constraints, and very
      easy to calculate the gradients of the objective and constraints.   I
      personally find it very nice to explicitly account for the parametric
      encoding of the trajectory in the solution technique.</p>

      <div data-type="example" class="drake"><h1>Direct Collocation for the
      Pendulum, Acrobot, and Cart-Pole</h1>

        <p>Direct collocation also easily solves the minimum-time problem for
        the double integrator.  The performance is similar to direct
        transcription, but the convergence is visibly different.  Try it for
        yourself:</p>

        <pysrc>pendulum/dircol_swingup.py</pysrc>

        <pysrc>acrobot/dircol_swingup.py</pysrc>

        <pysrc>cartpole/dircol_swingup.py</pysrc>

        As always, make sure you take a look at the code!

      </div>

    </section> <!-- end dircol -->

    <section data-type="sect2"><h1>Shooting Methods</h1>

      <p>In the methods described above, by asking the optimization package to
      perform the numerical integration of the equations of motion, we are
      effectively over-parameterizing the problem.  In fact, the optimization is
      perfectly well defined if we restrict the decision variables to
      $\bu_0,...,\bu_{N-1}$ only--we can compute $\bx_1,...,\bx_{N}$ ourselves
      by knowing $\bx_0$, and $\bu(\cdot)$.  This is exactly the approach taken
      in the shooting methods.</p>

      <section data-type="sect3"><h1>Computing gradients</h1>

        <p>Again, providing gradients of the objectives and constraints to the
        solver is not   strictly required -- most solvers will obtain them from
        finite differences if they   are not provided -- but I feel strongly
        that the solvers are faster and more robust when   exact gradients are
        provided.  Now that we have removed the $\bx$ decision variables   from
        the program, we have to take a little extra care to compute the
        gradients.  This   is still easily accomplished using the chain rule.
        To be concise (and slightly more general),   let us define
        $\bx[n+1]=f_d(\bx[n],\bu[n])$ as the discrete-time approximation of the
        continuous dynamics; for example, the forward Euler integration scheme
        used above would give   $f_d(\bx[n],\bu[n]) =
        \bx[n]+f(\bx[n],\bu[n])dt.$  Then we have   \[ \pd{J}{\bu_k} =
        \sum_{n=0}^{N-1} \left( \pd{g(\bx[n],\bu[n])}{\bx[n]} \pd{\bx[n]}{\bu_k}
        + \pd{g(\bx[n],\bu[n])}{\bu_k} \right), \]   where the gradient of the
        state with respect to the inputs can be computed   during the "forward
        simulation",   \[ \pd{\bx[n+1]}{\bu_k} = \pd{f_d(\bx[n],\bu[n])}{\bx[n]}
        \pd{\bx[n]}{\bu_k} + \pd{f_d(\bx[n],\bu[n])}{\bu_k}.  \]   These
        simulation gradients can   also be used in the chain rule to provide the
        gradients of any constraints.  Note that there are a lot of terms to
        keep around here, on the order of (state dim) $\times$ (control dim)
        $\times$ (number of timesteps).   Ouch.  Note also that many of these
        terms are zero; for instance with the Euler integration scheme above
        $\pd{\bu[n]}{\bu_k} = 0$ if $k\ne n$.  (If this looks like I'm mixing
        two notations here, recall that I'm using $\bu_k$ to represent the
        decision variable and $\bu[n]$ to represent the input used in the $n$th
        step of the simulation.)</p>

      </section>  <!-- gradients -->

      <section data-type="sect3"><h1>The special case of optimization without
      state constraints</h1>

        <p>By solving for $\bx(\cdot)$ ourselves, we've removed a large number
        of constraints from the optimization.  If no additional state
        constraints are present, and the only gradients we need to compute are
        the gradients of the objective, then a surprisingly efficient algorithm
        emerges.  I'll give the steps here without derivation, but will derive
        it in the Pontryagin section below: <ol> <li>Simulate forward:
        $$\bx[n+1] = f_d(\bx[n],\bu_n),$$ from  $\bx[0] = \bx_0$</li>
        <li>Calculate backwards: $$\lambda[n-1] =
        \pd{g(\bx[n],\bu[n])}{\bx[n]}^T + \pd{f(\bx[n],\bu[n])}{\bx[n]}^T
        \lambda[n],$$ from $\lambda[N-1]=0$</li> <li>Extract the gradients:
        $$\pd{J}{\bu[n]} = \pd{g(\bx[n],\bu[n])}{\bu[n]} + \lambda[n]^T
        \pd{f(\bx[n],\bu[n])}{\bu[n]},$$ with $\pd{J}{\bu_k} = \sum_n
        \pd{J}{\bu[n]}\pd{\bu[n]}{\bu_k}$.</li> </ol> <p>Here $\lambda[n]$ is a
        vector the same size as $\bx[n]$ which has an interpretation as
        $\lambda[n]=\pd{J}{\bx[n+1]}^T$.  The equation governing $\lambda$ is
        known as the <em>adjoint equation</em>, and it represents a dramatic
        efficient improvement over calculating the huge number of simulation
        gradients described above. In case you are interested, the adjoint
        equation is known as the <em>backpropagation algorithm</em> in the
        neural networks literature and it is one of the primary reasons that
        training neural networks became so popular in the 1980's; super fast GPU
        implementations of this algorithm are also one of the reasons that
        <em>deep learning</em> is incredibly popular right now (the availability
        of massive training databases is perhaps the other main reason). </p>

        <p>To take advantage of this efficiency, advocates of the shooting
        methods often use penalty methods   instead of enforcing hard state
        constraints; instead of telling the solver about   the constraint
        explicitly, you simply add an additional term to the cost function
        which gives a large penalty commensurate with the amount by which the
        constraint   is violated. These are not quite as accurate and can be
        harder to tune (you'd   like the cost to be high compared to other
        costs, but making it too high can   lead to numerical conditioning
        issues), but they can work.</p>

      </section>

    </section> <!-- end shooting -->

    <section data-type="sect2"><h1>Discussion</h1>

      <p>Although the decision about which algorithm is best may depend on the
      situation,   in our work we have come to favor the direct collocation
      method (and occasionally direct   transcription) for most of our work.
      There are a number of arguments for and   against each approach; I will
      try to discuss a few of them here.</p>

      <section data-type="sect3"><h1>Solver performance</h1>

        <p>Numerical conditioning.  Tail wagging the dog.</p>

        <p>Sparse constraints.  Potential for parallel evaluation of the
        constraints (computing the dynamics and their derivatives are often the
        most expensive part). </p>

      </section>

      <section data-type="sect3"><h1>Providing an initial guess</h1>

        <p>to avoid local minima.  direct transcription and collocation can take
        an initial guess in $\bx$, too.</p>

      </section>

      <section data-type="sect3"><h1>Implicit dynamics</h1>

        <p>Another potential advantage of the direct transcription and
        collocation methods is that the dynamics constraints can be written in
        implicit form.</p>

      </section>

      <section data-type="sect3"><h1>Variations in the problem formulation</h1>

        <p>There are number of useful variations to the problem formulations
        I've presented above. By far the most common is the addition of a
        terminal cost, e.g.: $$\min \quad h(\bx[N]) + \sum_{n=0}^{N-1}
        g(\bx[n],\bu[n]).$$ These terms are easily added to the cost function in
        the any of methods, and the adjoint equations of the shooting method
        simply require the a modified terminal condition $$\lambda[N-1] =
        \pd{h(\bx[N])}{\bx[N]}$$ when computing the gradients.</p>

        <p>Another common modification is including the spacing of the
        breakpoints as additional decision variables.  This is particularly easy
        in the direct transcription and collocation methods, and can also be
        worked into the shooting methods.  Typically we add a lower-bound on the
        time-step so that they don't all vanish to zero.</p>

      </section>

      <section data-type="sect3"><h1>Accuracy of numerical integration</h1>

        <p>One potential complaint about the direct transcription and
        collocation algorithms is that we tend to use simplistic numerical
        integration methods and often fix the integration timestep (e.g. by
        choosing Euler integration and selecting a $dt$); it is difficult to
        bound the resulting integration errors in the solution.  One tantalizing
        possibility in the shooting methods is that the forward integration
        could be accomplished by more sophisticated methods, like variable-step
        integration.  But I must say that I have not had much success with this
        approach, because while the numerical accuracy of any one function
        evaluation might be improved, these integration schemes do not
        necessarily give smooth outputs as you make incremental changes to the
        initial conditions and control (changing $\bu_2$ by $\epsilon$ could
        result in taking a different number of steps in the integration scheme).
        This lack of smoothness can wreak havoc on the convergence of the
        optimization. If numerical accuracy is a premium, then I think you will
        have more success by imposing consistency constraints (e.g. as in the
        Runge-Kutta 4th order simulation with 5th order error checking method)
        as additional constraints on the time-steps; shooting methods do not
        have any particular advantage here.</p>

        <todo>reference DMOC</todo>

      </section> <!-- end accuracy -->

    </section> <!-- end pros/cons -->

  </section> <!-- end three algorithms -->

  <section data-type="sect1"><h1>Pontryagin's Minimum Principle</h1>

    <p>The tools that we've been developing for numerical trajectory
    optimization are closely tied to theorems from (analytical) optimal control.
     Let us take one section to appreciate those connections.</p>

    <p>What precisely does it mean for a trajectory, $\bx(\cdot),\bu(\cdot)$, to
    be locally optimal?  It means that if I were to perturb that trajectory in
    any way (e.g. change $\bu_3$ by $\epsilon$), then I would either incur
    higher cost in my objective function or violate a constraint.  For an
    unconstrained optimization, a <em>necessary condition</em> for local
    optimality is that the gradient of the objective at the solution be exactly
    zero.  Of course the gradient can also vanish at local maxima or saddle
    points, but it certainly must vanish at local minima. We can generalize this
    argument to constrained optimization using <em>Lagrange multipliers</em>.
    </p>

    <section data-type="sect2"><h1>Constrained optimization with Lagrange
    multipliers</h1>

      <p>Given the equality-constrained optimization problem $$\minimize_\bz
      g(\bz) \quad \subjto \quad \bphi(\bz) = 0,$$ where $\bphi$ is a vector.
      Define a vector $\lambda$ of Lagrange multipliers, the same size as
      $\phi$, and the scalar Lagrangian function, $$L(\bz,{\bf \lambda}) =
      g(\bz) + \lambda^T\phi(\bz).$$ A necessary condition for $\bz^*$ to be an
      optimal value of the constrained optimization is that the gradients of $L$
      vanish with respect to both $\bz$ and $\lambda$: $$\pd{L}{\bz} = 0, \quad
      \pd{L}{\lambda} = 0.$$  Note that $\pd{L}{\lambda} = \phi(\bz)$, so
      requiring this to be zero is equivalent to requiring the constraints to be
      satisfied.</p>

      <div data-type="example"><h1>Optimization on the unit circle</h1>

        <p>Consider the following optimization: $$\min_{x,y} x+y, \quad \subjto
        \quad x^2 + y^2 = 1.$$ The level sets of $x+y$ are straight lines with
        slope $-1$, and the constraint requires that the solution lives on the
        unit circle.</p> <figure> <img width="80%"
        data-src="figures/lagrange_multipliers_unit_circle.svg"/> </figure>
        <p>Simply by inspection, we can determine that the optimal solution
        should be $x=y=-\frac{\sqrt{2}}{2}.$ Let's make sure we can obtain the
        same result using Lagrange multipliers. </p>

        <p>Formulating $$L = x+y+\lambda(x^2+y^2-1),$$ we can take the
        derivatives and solve

        \begin{align*}
        \pd{L}{x} = 1 + 2\lambda x = 0 \quad & \Rightarrow & \lambda = -\frac{1}{2x}, \\
        \pd{L}{y} = 1 + 2\lambda y = 1 - \frac{y}{x} = 0 \quad & \Rightarrow & y = x,\\
        \pd{L}{\lambda} = x^2+y^2-1 = 2x^2 -1 = 0 \quad & \Rightarrow & x = \pm \frac{1}{\sqrt{2}}.
        \end{align*}

        Given the two solutions which satisfy the necessary conditions, the
        negative solution is clearly the minimizer of the objective.</p>

      </div>

    </section>

    <section data-type="sect2"><h1>Lagrange multiplier derivation of the adjoint equations</h1>

      <p>Let us use Lagrange multipliers to derive the necessary conditions for
      our constrained trajectory optimization problem in discrete time
      \begin{align*} \minimize_{\bx_1,...,\bx_N,\bu_0,...,\bu_{N-1}} &
      \sum_{n=0}^{N-1} g(\bx[n],\bu[n]),\\ \subjto \quad & \bx[n+1] =
      f_d(\bx[n],\bu[n]). \end{align*} Formulate the Lagrangian, \[
      L(\bx[\cdot],\bu[\cdot],\lambda[\cdot]) = \sum_{n=0}^{N-1}
      g(\bx[n],\bu[n]) + \sum_{n=0}^{N-1} \lambda^T[n] \left(f_d(\bx[n],\bu[n])
      - \bx[n+1]\right), \] and set the derivatives to zero to obtain the
      adjoint equation method described for the shooting algorithm above:
      \begin{gather*} \forall n\in[0,N-1], \pd{L}{\lambda[n]} =
      f_d(\bx[n],\bu[n]) - \bx[n+1] = 0 \Rightarrow \bx[n+1] = f(\bx[n],\bu[n])
      \\ \forall n\in[0,N-1], \pd{L}{\bx[n]} = \pd{g(\bx[n],\bu[n])}{\bx} +
      \lambda^T[n] \pd{f_d(\bx[n],\bu[n])}{\bx} - \lambda^T[n-1] = 0 \\ \quad
      \Rightarrow \lambda[n-1] = \pd{g(\bx[n],\bu[n])}{\bx}^T +
      \pd{f_d(\bx[n],\bu[n])}{\bx}^T \lambda[n]. \\ \pd{L}{\bx[N]} =
      -\lambda[N-1] = 0 \Rightarrow \lambda[N-1] = 0 \\ \forall n\in[0,N-1],
      \pd{L}{\bu[n]} = \pd{g(\bx[n],\bu[n])}{\bu} + \lambda^T[n]
      \pd{f_d(\bx[n],\bu[n])}{\bu} = 0. \end{gather*} Therefore, if we are given
      an initial condition $\bx_0$ and an input trajectory $\bu[\cdot]$, we can
      verify that it satisfies the necessary conditions for optimality by
      simulating the system forward in time to solve for $\bx[\cdot]$, solving
      the adjoint equation backwards in time to solve for $\lambda[\cdot]$, and
      verifying that $\pd{L}{\bu[n]} = 0$ for all $n$. The fact that
      $\pd{J}{\bu} = \pd{L}{\bu}$ when $\pd{L}{\bx} = 0$ and $\pd{L}{\lambda} =
      0$ follows from some basic results in the calculus of variations.</p>

    </section>

    <section data-type="sect2"><h1>Necessary conditions for optimality in
    continuous time</h1>

      <p>You won't be surprised to hear that these necessary conditions have an
      analogue in continuous time. I'll state it here without derivation.  Given
      the initial conditions, $\bx_0$, a continuous dynamics, $\dot\bx =
      f(\bx,\bu)$, and the instantaneous cost $g(\bx,\bu)$, for a trajectory
      $\bx(\cdot),\bu(\cdot)$ defined over $t\in[t_0,t_f]$  to  be optimal it
      must satisfy the conditions that \begin{align*} \forall
      t\in[t_0,t_f],\quad & \dot{\bx} = f(\bx,\bu), \quad &\bx(0)=\bx_0\\
      \forall t\in[t_0,t_f],\quad & -\dot\lambda = \pd{g}{\bx}^T + \pd{f}{\bx}^T
      \lambda, \quad &\lambda(T) = 0\\ \forall t\in[t_0,t_f],\quad & \pd{g}{\bu}
      + \lambda^T\pd{f}{\bu} = 0.& \end{align*}</p>

      <p>In fact the statement can be generalized even beyond this to the case
      where $\bu$ has constraints. The result is known as Pontryagin's minimum
      principle -- giving <em>necessary conditions</em> for a trajectory to be
      optimal.</p>P

      <div data-type="theorem"><h1>Pontryagin's Minimum Principle</h1>

        <p>Adapted from <elib>Bertsekas00a</elib>. Given the initial conditions,
        $\bx_0$, a continuous dynamics, $\dot\bx = f(\bx,\bu)$, and the
        instantaneous cost $g(\bx,\bu)$, for a trajectory
        $\bx^*(\cdot),\bu^*(\cdot)$ defined over $t\in[t_0,t_f]$  to  be optimal
        is must satisfy the conditions that \begin{align*} \forall
        t\in[t_0,t_f],\quad & \dot{\bx}^* = f(\bx^*,\bu^*), \quad
        &\bx^*(0)=\bx_0\\ \forall t\in[t_0,t_f],\quad & -\dot\lambda =
        \pd{g}{\bx}^T + \pd{f}{\bx}^T \lambda, \quad &\lambda(T) = 0\\ \forall
        t\in[t_0,t_f],\quad & u^* = \argmin_{\bu\in{\cal U}} \left[ g(\bx^*,\bu)
        + \lambda^T f(\bx^*,\bu) \right].& \end{align*}</p>

      </div>

      <p>Note that the terms which are minimized in the final line of the
      theorem are commonly referred to as the Hamiltonian of the optimal control
      problem, $$H(\bx,\bu,\lambda,t) = g(\bx,\bu) + \lambda^T f(\bx,\bu).$$ It
      is distinct from, but inspired by, the Hamiltonian of classical mechanics.
      Remembering that $\lambda$ has an interpretation as $\pd{J}{\bx}^T$, you
      should also recognize it from the HJB. </p>

    </section>
  </section> <!-- end pontryagin -->

  <section data-type="sect1"><h1>Trajectory optimization as a convex
  optimization</h1>

    <section data-type="sect2"><h1>Linear systems with convex linear
    constraints</h1>

      <p>An important special case.  Linear/Quadratic objectives results in an
      LP/QP $\Rightarrow$ Convex optimization.</p>

    </section>

    <section data-type="sect2"><h1>Differential Flatness</h1>

    </section>

    <section data-type="sect2"><h1>Mixed-integer convex optimization for
    non-convex constraints</h1>

    </section>

  </section>

  <section data-type="sect1"><h1>Local Trajectory Feedback Design</h1>

    <p>Once we have obtained a locally optimal trajectory from trajectory
    optimization, there is still work to do...</p>

    <section data-type="sect2"><h1>Model-predictive control</h1>

    </section>

    <section data-type="sect2"><h1>Time-varying LQR</h1>

      <p>Take $\bar\bx(t) = \bx(t) - \bx_0(t)$, and $\bar\bu(t) =
      \bu(t)-\bu_0(t)$, then apply finite-horizon LQR (see the LQR chapter).</p>

    </section>

  </section> <!-- end feedback design -->

  <section data-type="sect1"><h1>Iterative LQR and Differential Dynamic
  Programming</h1>

  </section>

  <section data-type="sect1"><h1>Case Study: A glider that can land on a perch
  like a bird</h1>

  </section>

  <section
      data-type="sect1"><h1>Trajectory optimization as policy search.</h1>

    <p>Can also parameterize a feedback controller and search over those
      parameters directly (instead of parameterizing the input tape).</p>

  </section>

</section> <!-- ***************  end of trajectory optimization **************   -->

<section data-type="chapter" class="chapter" id="limit_cycles"><h1>Algorithms
for Limit Cycles</h1>

  <p>The discussion of walking and running robots in Chapter 4 motivated the
  notion of limit cycle stability.  Linear systems are not capable of producing
  stable limit cycle behavior, so this rich topic is unique to nonlinear systems
  design and analysis.  Furthermore, the tools that are required to design,
  stabilize, and verify limit cycles will have applicability beyond simple
  periodic motions.</p>

  <p>The first natural question we must ask is, given a system $\dot{\bx} =
  f(\bx)$, or a control system $\dot{x} = f(\bx,\bu)$, how do we go about
  finding periodic solutions which may be passively stable, open-loop stable, or
  stabilized with closed-loop feedback? It turns out that the trajectory
  optimization tools that we developed in the last chapter are very well suited
  to this task.</p>

  <section data-type="sect1"><h1>Trajectory optimization</h1>

    <p>I introduced the trajectory optimization tools as means for optimizing a
    single trajectory of the system from a particular known initial condition.
    But they can be used in a slightly different way, too -- in some
    formulations we might choose to have the initial conditions as one of the
    decision variables.  In fact, the simplest formulation for finding a
    periodic solution is to leave the entire trajectory as a decision variable,
    but to add <em>periodicity constraints</em> enforcing that the initial
    conditions and the final conditions match.  Some care is needed here; if
    you're not careful then these constraints will all be met if the entire
    trajectory remains at a fixed point in the system.  In the example below, we
    constrain that the initial and final conditions lie on the surface of
    section that we used before, and additionally constrain that the velocity at
    the initial/final points is non-zero (to avoid the fixed point
    solution).</p>

    <p>Note that although we introduced trajectory optimization as searching
    over control inputs $\bu$, this formulation is now rich enough that it
    solves an important problem even for passive systems -- with no control
    input at all.</p>

    <div data-type="example" class="drake"><h1>Finding the limit cycle of the
    Van der Pol oscillator</h1>

      <p>Recall the dynamics of the Van der Pol oscillator given by $$\ddot{q} +
      \mu (q^2 - 1) \dot{q} + q = 0, \quad \mu>0,$$ which exhibited a stable
      limit cycle.</p>

      <p>Formulate the direct collocation optimization:
        \begin{align*}
          \find_{\bx_0,...,\bx_N,dt} \quad \\
          \subjto \quad    & q_0 = 0, \quad \dot{q}_0 > 0, \\
          & \bx_N = \bx_0, \text{(periodicity constraint)}\\
          & \text{collocation dynamic constraints}
        \end{align*}
      </p>

      <p>Try it yourself:</p>
        <pysrc>van_der_pol/dircol.py</pysrc>

      <p>As always, make sure that you take a look at the code.  Poke around.
      Try changing some things.</p>

      <p>One of the things that you should notice in the code is that I provide
      an initial guess for the solver.  In most of the examples so far I've been
      able to avoid doing that--the solver takes small random numbers as a
      default initial guess and solves from there.  But for this problem, I
      found that it was getting stuck in a local minima.  Adding the initial
      guess that the solution moves around a circle in state space was enough.
      </p>

    </div>

    <todo> example: simplest flapping robot </todo>

  </section> <!-- end trajectory optimization -->

  <section data-type="sect1"><h1>Lyapunov analysis</h1>

    <p>Recall the important distinction between stability of a trajectory in
    time and   stability of a limit cycle was that the limit cycle does not
    converge in phase --   trajectories near the cycle converge to the cycle,
    but trajectories on the cycle   may not converge with each other.  This is
    type of stability, also known as <em>orbital   stability</em> can be written
    as stability to the manifold described by cycle   $\bx^*(t)$, \[ \min_\tau
    || x(t) - x^*(\tau) || \rightarrow 0.\] In the case of limit cycles, this
    manifold is a periodic solution with $\bx^*(t+t_{period}) = \bx^*(t)$.
    Depending on exactly how that convergence happens, we can define orbital
    stability in the sense of Lyapunov, asymptotic orbital stability,
    exponential orbital stability, or even finite-time orbital stability.</p>

    <p>In order to prove that a system is orbitally stable (locally, over a
    region, or globally), or to analyze the region of attraction of a limit
    cycle, we can use a Lyapunov function. In particular, we would like to
    consider Lyapunov functions which have the form cartooned below;  they
    vanish to zero everywhere along the cycle, and are strictly positive
    everywhere away from the cycle.</p>

    <figure><img
    data-src="figures/limitCycleLyapunovCartoon.svg"/><figcaption>Cartoon of a
    Lyapunov function which vanishes on a limit cycle, and is strictly positive
    everywhere else. (a small segment has been removed solely for the purposes
    of visualization).</figcaption></figure>

    <section data-type="sect2"><h1>Transverse coordinates</h1>

      <p>How can we parameterize this class of functions?  For arbitrary cycles
      this could be very difficult in the original coordinates.  For simple
      cycles like in the cartoon, one could imagine using polar coordinates.
      More generally, we will define a new coordinate system relative to the
      orbit, with coordinates</p> <ul> <li>$\tau$ - the phase along the
      orbit</li> <li>$\bx_\perp(\tau)$ - the remaining coordinates, linearly
      independent from $\tau$.</li> </ul> <p>Given a state $\bx$ in the original
      coordinates, we must define a smooth mapping $\bx \rightarrow (\tau,
      \bx_\perp)$ to this new coordinate system.  For example, for a simple ring
      oscillator we might have:</p> <figure><img width="80%"
      data-src="figures/transverseCoordinates.svg"/><figcaption>A moving
      coordinate system along the limit cycle.</figcaption></figure> In general,
      for an $n$-dimensional state space, $\tau$ will always be a scalar, and
      $\bx_\perp$ will be an $(n-1)$-dimensional vector defining the remaining
      coordinates relative to $\bx^*(\tau)$. In fact, although we use the
      notation $\bx_\perp$ the coordinate system need not be strictly orthogonal
      to the orbit, but must simply be <em>transversal</em> (not parallel).
      Having defined the smooth mapping $\bx \rightarrow (\tau,\bx_\perp)$, we
      can always rewrite the dynamics in this new coordinate system:
      \begin{gather*} \dot{\tau} = f_1(\bx_\perp,\tau) \\ \dot\bx_\perp =
      f_2(\bx_\perp,\tau). \end{gather*} <p>The value of this construction for
      Lyapunov analysis was proposed in <elib>Hauser94a</elib> and has been
      extended nicely to control design in <elib>Shiriaev08</elib> and for
      region of attraction estimation in <elib>Manchester10b</elib>.  A
      particular numerical strategy for defining the transversal coordinates is
      given in <elib>Manchester10a</elib>.</p>

      <div data-type="theorem"><h1>A Lyapunov theorem for orbital stability</h1>

        <p>For a dynamical system $\dot\bx = f(\bx)$ with $\bx \in \Re^n$, $f$
        continuous, a continuous periodic solution $\bx^*(\tau)$, and a smooth
        mapping $\bx \rightarrow (\tau,\bx_\perp)$ where $\bx_\perp$ vanishes on
        $\bx^*$, then for some $n-1$ dimensional ball ${\cal B}$ around the
        origin, if I can produce a $V(\bx_\perp,\tau)$ such that \begin{gather*}
        \forall \tau, V(0,\tau) = 0, \\ \forall \tau, \forall \bx_\perp \in
        {\cal B}, \bx_\perp \ne 0, V(\bx_\perp,\tau) > 0, \end{gather*} with
        \begin{gather*} \forall \tau, \dot{V}(0,\tau) = 0, \\ \forall \tau,
        \forall \bx_\perp \in {\cal B}, \bx_\perp \ne 0, \dot{V}(\bx_\perp,\tau)
        < 0, \end{gather*} then the solution $\bx^*(t)$ is <em>locally orbitally
        asymptotically stable</em>.</p>

      </div>

      <div data-type="example"><h1>Simple ring oscillator</h1>

        <p> Perhaps the simplest oscillator is the first-order system which
        converges to the unit circle.   In cartesian coordinates, the dynamics
        are   \begin{align*} \dot{x}_1 =& x_2 -\alpha x_1 \left( 1 -
        \frac{1}{\sqrt{x_1^2+x_2^2}}\right) \\ \dot{x}_2 =& -x_1 -\alpha x_2
        \left( 1 - \frac{1}{\sqrt{x_1^2+x_2^2}}\right), \end{align*}   where
        $\alpha$ is a positive scalar gain.</p>

        <figure><img width=80%
        data-src="figures/ringOscillator.svg"/><figcaption>Vector field of the
        ring oscillator</figcaption></figure>

        <p>If we take the transverse coordinates to be the polar coordinates,
        shifted so that $x_\perp$ is zero on the unit circle, \begin{align*}\tau
        =& \text{atan2}(-x_2,x_1) \\ x_\perp =& \sqrt{x_1^2+x_2^2}-1,
        \end{align*} which is valid when $x_\perp>-1$, then the simple
        transverse dynamics are revealed:   \begin{align*}     \dot\tau =& 1 \\
        \dot{x}_\perp =& -\alpha x_\perp.   \end{align*}   Taking a Lyapunov
        candidate $V(x_\perp,\tau) = x_\perp^2$, we can verify that $$\dot{V} =
        -2 \alpha x_\perp^2 \prec 0, \quad \forall x_\perp > -1.$$ This
        demonstrates that the limit cycle is locally asymptotically stable, and
        furthermore that the invariant open-set $V < 1$ is inside the region of
        attraction of that limit cycle. In fact, we know that all $x_\perp > -1$
        are in the region of attraction that limit cycle, but this is not proven
        by the Lyapunov argument above. </p>

      </div>

      <p>Let's compare this approach with the approach that we used in the
      chapter on walking robots, where we used a Poincar&eacute map analysis to
      investigate limit cycle stability. In transverse coordinates approach,
      there is an additional burden to construct the coordinate system along the
      entire trajectory, instead of only at a single surface of section.  In
      fact, the transverse coordinates approach is sometimes referred to as a
      "moving Poincar&eacute section". But the reward for this extra work is
      that we can check a condition that only involves the instantaneous
      dynamics, $f(\bx)$, as opposed to having to integrate the dynamics over an
      entire cycle to generate the discrete Poincar&eacute map, $\bx_p[n+1] =
      P(\bx_p[n])$. While computing $P$ in closed-form happened to be possible
      for the simple rimless wheel model, it is rarely possible for more complex
      models.  As we will see below, this approach will also be more compatible
      with designing continuous feedback controller that stabilize the limit
      cycle.</p>

    </section> <!-- end transverse -->

    <section data-type="sect2"><h1>Transverse linearization</h1>

      <p>In the case of Lyapunov analysis around a fixed point, there was an
      important special case: for stable linear systems, we actually have a
      recipe for constructing a Lyapunov function.  As a result, for nonlinear
      systems, we often found it convenient to begin our search by linearizing
      around the fixed point and using the Lyapunov candidate for the linear
      system as an initial guess.  That same approach can be extended to limit
      cycle analysis.</p>

    </section>

    <section data-type="sect2"><h1>Region of attraction estimation using sums-of-squares</h1>

      <p>Coming soon.  If you are interested, see <elib>Manchester10a+Manchester10b</elib>.</p>

    </section>

  </section> <!-- end lyapunov -->

  <section data-type="sect1"><h1>Feedback design</h1>

    <section data-type="sect2"><h1>Transverse LQR</h1>

    </section>

    <section data-type="sect2"><h1>Orbital stabilization for non-periodic trajectories</h1>

    </section>

  </section> <!-- end feedback -->

</section> <!-- end limit cycles -->

<section data-type="chapter" class="chapter" id="contact"><h1>Planning and
  Control
  through Contact</h1>

<section data-type="sect1"><h1>The hybrid system's approach</h1>

  <p>Transverse stabilization is more natural than full LQR

</section>

<section data-type="sect1"><h1>The measure differential inclusions approach</h1>
</section>

</section>

<!-- ***************  end of hybrid systems **************   -->

<section data-type="chapter" class="chapter" id="search"><h1>Motion Planning as
  Search</h1>

<p>The term "motion planning" is a hopelessly general term which almost certainly
  encompasses the dynamic programming, feedback design, and trajectory optimization
  algorithms that we have already discussed.  However, there are a number of algorithms
  and ideas that we have not yet discussed which have grown from the idea of formulating
  motion planning as a search problem -- for instance searching for a path from a start
  to a goal in a graph which is too large solve completely with dynamic programming.
  Some, but certainly not all, of these algorithms sacrifice optimality in order to
  find any path if it exists, and the notion of a planner being "complete" -- guaranteed to find
  a path if one exists -- is highly valued.  This is precisely our goal for this chapter,
  to add some additional tools that will be able to provide some form of solutions for our
  most geometrically complex, highly non-convex, robot control problems.</p>

<p><elib>LaValle06</elib> is a very nice book on planning algorithms in general and on
  motion planning algorithms in particular.  Compared to other planning problems,
  <em>motion planning</em> typically refers to problems where the planning domain is continuous
  (e.g. continuous state space, continuous action space), but many motion planning
algorithms trace their origins back to ideas in discrete domains (e.g., graph search).</p>

<p>For this chapter, we will consider the following problem formulation: given a system
defined by the nonlinear dynamics (in continuous- or discrete-time) $$\dot{\bx} = f(\bx,\bu) \quad \text{or} \quad \bx[n+1] = f(\bx[n],\bu[n]),$$ and given a start state
$\bx(0) = \bx_0$ and a goal region ${\cal G}$, find any finite-time trajectory from $\bx_0$ to
to $\bx \in {\cal G}$ if such a trajectory exists.</p>


<section data-type="sect1"><h1>Artificial Intelligence as Search</h1>

<p>A long history...  some people feel that the route to creating intelligent machines
is to collect large ontologies of knowledge, and then perform very efficient search.
(The more modern view of AI is focused instead on machine learning, the right answer
probably involves pieces of both.)  Samuels checker players, Deep Blue playing chess, theorem
proving, Cyc, IBM Watson,...</p>

<p>One of the key ideas is the use of "heuristics" to guide the search.  "Is it possible to find
  and optimal path from the start to a goal without visiting every node?".  $A^*$.  Admissible heuristics.
Example: google maps.</p>

<p>Online planning.  $D^*$, $D^*$-Lite.</p>

</section>

<section data-type="sect1"><h1>Randomized motion planning</h1>

<p>If you remember how we introduced dynamic programming initially as a graph search,
you'll remember that there were some challenges in discretizing the state space.  Let's assume
that we have discretized the continuous space into some finite set of discrete nodes
in our graph.  Even if
we are willing to discretize the action space for the robot (this might be even be acceptable
in practice), we had a problem where discrete actions from one node in the graph, integrated
over some finite interval $h$, are extremely unlikely to land exactly on top of another
node in the graph.  To combat this, we had to start working on methods for interpolating the value
function estimate between nodes.</p>

<todo>add figure illustrating the interpolation here</todo>

<p>Interpolation can work well if you are trying to solve for the cost-to-go function over the entire
state space, but it's less compatible with search methods which are trying to find
just a single path through the space.  If I start in node 1, and land between node 2 and node 3, then
which node to I continue to expand from?</p>

  <p>One approach to avoiding this problem is to build a <em>search tree</em> as the search executes, instead of relying on a
  predefined mesh discretization.  This tree will contains nodes rooted in the continuous space at exactly
the points where system can reach.</p>

<p>Another other problem with any fixed mesh discretization of a continuous space, or even
  a fixed discretization of the action space, is that
unless we have specific geometric / dynamic insights into our continuous system, it very difficult
to provide a <em>complete</em> planning algorithm.  Even if we can show that no path to the goal exists on
the tree/graph, how can we be certain that there is no path for the continuous system?  Perhaps
a solution would have emerged if we had discretized the system differently, or more finely?</p>

<p>One approach to addressing this second challenge is to toss out the notion of fixed
discretizations, and replace them with random sampling (another approach would be to adaptively
add resolution to the discretization as the algorithm runs).  Random sampling, e.g. of the action
space, can yield algorithms that are <em>probabilistically complete</em> for the continuous space --
if a solution to the problem exists, then a probabilistically complete algorithm will
find that solution with probability 1 as the number of samples goes to infinity.</p>

<p>With these motivations in mind, we can build what is perhaps the simplest probabilistically
complete algorithm for finding a path from the a starting state to some goal region with
in a continuous state and action space:</p>

<div data-type="example" class="drake"><h1>Planning with a Random Tree</h1>

<p>Let us denote the data structure which contains the tree as ${\cal T}$.  The algorithm is very simple:
  <ul>
    <li>Initialize the tree with the start state: ${\cal T} \leftarrow \bx_0$.</li>
    <li>On each iteration:
      <ul>
        <li>Select a random node, $\bx_{rand}$, from the tree, ${\cal T}$</li>
        <li>Select a random action, $\bu_{rand}$, from a distribution over feasible actions.</li>
        <li>Compute the dynamics: $\bx_{new} = f(\bx_{rand},\bu_{rand})$</li>
        <li>If $\bx_{new} \in {\cal G}$, then terminate. Solution found!</li>
        <li>Otherwise add the new node to the tree, ${\cal T} \leftarrow \bx_{new}$.</li>
      </ul>
    </li>
  </ul>
  It can be shown that this algorithm is, in fact, probabilistically complete.  However,
  without strong heuristics to guide the selection of the nodes scheduled for expansion,
  it can be extremely inefficient.  For a simple example, consider the system $\bx[n] = \bu[n]$ with $\bx \in \Re^2$ and $\bu_i \in [-1,1]$.
  We'll start at the origin and put the goal region as $\forall i, 15 \le x_i \le 20$.
  Try it yourself:

  <pre><code class="matlab" testfile="testRandomTree">
T = struct('parent',zeros(1,1000),'node',zeros(2,1000));  % pre-allocate memory for the "tree"
for i=2:size(T.parent,2)
  T.parent(i) = randi(i-1);
  x_rand = T.node(:,T.parent(i));
  u_rand = 2*rand(2,1)-1;
  x_new = x_rand+u_rand;
  if (15<=x_new(1) && x_new(1)<=20 && 15<=x_new(2) && x_new(2)<=20)
    disp('Success!'); break;
  end
  T.node(:,i) = x_new;
end
clf;
line([T.node(1,T.parent(2:end));T.node(1,2:end)],[T.node(2,T.parent(2:end));T.node(2,2:end)],'Color','k');
patch([15,15,20,20],[15,20,20,15],'g')
axis([-10,25,-10,25]);
  </code></pre>
  Again, this algorithm <em>is</em> probabilistically complete.  But after expanding 1000 nodes, the tree is basically a mess of node points all right on top of
  each other:
  <figure>
    <img width="70%" data-src="figures/random_tree.svg"/>
  </figure>
  <p>We're nowhere close to the goal yet, and it's not exactly a hard problem.
</p>

</div>

<p>
While the idea of generating a tree of feasible points has clear advantages, we have lost
the ability to cross off a node (and therefore a region of space) once it has been explored.
It seems that, to make randomized algorithms effective, we are going to at the very least need some
form of heuristic for encouraging the nodes to spread out and explore the space.
</p>

<section data-type="sect2"><h1>Rapidly-Exploring Random Trees (RRTs)</h1>

<figure>
  <img width="70%" data-src="figures/rrt.svg"/>
</figure>

  <figure>
    <img width="90%" data-src="figures/rrt_basic.svg"/>
    <figcaption>(<a href="figures/rrt_basic.swf">Click here to watch the animation</a>)</figcaption>
  </figure>

  <figure>
    <img width="90%" data-src="figures/rrt_voronoi.svg"/>
    <figcaption>(<a href="figures/rrt_voronoi.swf">Click here to watch the animation</a>)</figcaption>
</figure>


</section>

<section data-type="sect2"><h1>RRTs for robots with dynamics</h1>

</section>

<section data-type="sect2"><h1>Variations and extensions</h1>

<p>Multi-query planning with PRMs, ...
</p>

<p>RRT*, RRT-sharp, RRTx,...  </p>

<p>Complexity bounds and dispersion limits</p>

</section>

<section data-type="sect2"><h1>Discussion</h1>

<p>Not sure yet whether randomness is fundamental here, or whether is a temporary "crutch"
until we understand geometric and dynamic planning better.</p>

</section>
</section>

<section data-type="sect1"><h1>Decomposition methods</h1>

<p>Cell decomposition...</p>

<p>Mixed-integer planning.</p>

<p>Approximate decompositions for complex environments (e.g. IRIS)</p>

</section>
</section>

<!-- ***************  end of feasible motion planning **************   -->



<section data-type="chapter" class="chapter"
         id="feedback_motion_planning"><h1>Feedback Motion
  Planning</h1>

</section>

<!-- ***************  end of feedback motion planning **************   -->

<section data-type="chapter" class="chapter" id="output"><h1>Output
  Feedback</h1>

</section>


<!-- ***************  end of output feedback **************   -->

<section data-type="chapter" class="chapter" id="robust"><h1>Robust and
  Stochastic
  Control</h1>

<todo>
Stochastic optimal control.  Stochastic HJB.  Graph search.  LQR + Gaussian noise.  Whitening filters.
Stochastic trajectory optimization.

Stochastic verification. (Having bounds on expected value does yield bounds on system state).


Robust control.  Common Lyapunov functions.  Dissipation inequalities.  Guaranteed-cost control for linear systems. H-infinity.
</todo>

</section>

<!-- ***************  end of robust control **************   -->

<section data-type="chapter" class="chapter" id="belief"><h1>Planning Under
Uncertainty</h1>

  <todo>POMDP, SLAG, Belief-space sample-based planning, ...</todo>

</section>

<!-- ***************  end of planning under uncertainty **************   -->


<section data-type="chapter" class="chapter" id="verification"><h1>Verification
and Validation</h1>

  <todo>Lyap w/ richer specs.  Monte carlo... .</todo>

</section>

<!-- ***************  end of verification **************   -->


</div> <!-- end of Part II : Nonlinear Planning and Control -->

<!-- START OF PART III - Estimation and Learning -->
<div class="part" title="Estimation and Learning">

<section data-type="chapter" class="chapter"
         id="sysid"><h1>System Identification</h1>

  <todo>equation error vs simulation error.  model-reduction for linear systems.  optimal experiment design</todo>

</section>

<section data-type="chapter" class="chapter"
         id="estimation"><h1>State Estimation</h1>

</section>

<section data-type="chapter" class="chapter"
         id="policy_search"><h1>Model-Free Policy Search</h1>

  <p>Reinforcement learning (RL) is a collection of algorithms for solving the
  same optimal control problem that we've focused on through the text, but the
  real gems from the RL literature are the algorithms for (almost) <i>black-box
  optimization</i> of stochastic optimal control problems. The idea of an
  algorithm that only has a "black-box" interface to the optimization problem
  means that it can obtain (potentially noisy) samples of the optimal cost via
  trial and error, but does not have access to the underlying model, and does
  not have direct access to complete gradient information.</p>

  <p>This is a hard problem!   In general we cannot expect RL algorithms to
  optimize as quickly as more structured optimization, and we can typically only
  guarantee convergence to a local optima at best.  But the framework is
  extremely general, so it can be applied to problems that are inaccessible to
  any of the other algorithms we've examined so far.  My favorite examples for
  reinforcement learning are control in complex fluid dynamics (e.g.
  <elib>Roberts09a</elib>). These systems are often very difficult to model, or
  the model is so high-dimensional and complicated so that it's prohibitive for
  control design. In these problems, it might actually be faster to optimize via
  trial-and-error in physical experiments. </p>

  <p>In this chapter we will examine one particular style of reinforcement
  learning that attempts to find good controllers by explicitly parameterizing a
  family of policies (e.g. via a parameter vector $\alpha$), then searching
  directly for the parameters that optimize the long-term cost.  For a
  stochastic optimal control problem with our favorite additive form of the
  objective, this might look like: \begin{equation} \min_\alpha E \left[
    \sum_{n=0}^N
  g(\bx[n],\bu[n]) \right]\end{equation} where the random variables are drawn
    from
  probability densities \begin{gather*}\bx[0] \sim p_0(\bx),\\ \bx[n] \sim
  p(\bx[n] | \bx[n-1], \bu[n-1]), \\ \bu[n] \sim p_\alpha(\bu[n] |
  \bx[n]).\end{gather*}  The last equation is a probabilistic representation of
  the control policy -- on each time-step actions $\bu$ are drawn from a
  distribution that is conditioned on the current state, $\bx$.</p>

  <p>Of course, the controls community has investigated ideas like this, too,
  for instance under the umbrellas of <i>extremum-seeking control</i> and
  <i>iterative learning control</i>.  I'll try to make connections whenever
  possible.</p>

  <section data-type="sect1"><h1>Policy Gradient Methods</h1>

    <p>One of the standard approaches to policy search in RL is to estimate the
    gradient of the expected long-term cost with respect to the policy
    parameters by evaluating some number of sample trajectories, then performing
    (stochastic) gradient descent.  Many of these so-called "policy gradient"
    algorithms leverage a derivation that was first described in the context of
    the REINFORCE algorithm <elib>Williams92</elib>.  It is based on what looks
    like a trick with logarithms to estimate the gradient; I feel like this
    trick is often presented with an air of mystery about it.  Let's try to make
    sure we understand it.</p>

    <section data-type="sect2"><h1>The Policy Gradient "Trick" (aka
      REINFORCE)</h1>

      <p>Let's start with a simpler optimization over a stochastic function:
      $$\min_\alpha E\left[ g(\bx) \right] \text{ with } \bx \sim p_\alpha(\bx)
      $$  I hope the notation is clear?  $\bx$ is a random vector, drawn from
      the distribution $p_\alpha(\bx)$, with the subscript indicating that the
      distribution depends on the parameter vector $\alpha$.  What is the
      gradient of this function?  The REINFORCE derivation is \begin{align*}
      \frac{\partial}{\partial \alpha} E \left[ g(\bx) \right] =&
      \frac{\partial}{\partial \alpha} \int d\bx ~ g(\bx) p_\alpha(\bx) \\ =&
      \int  d\bx ~ g(\bx) \frac{\partial}{\partial \alpha} p_\alpha(\bx)  \\ =&
      \int d\bx ~ g(\bx) p_\alpha(\bx) \frac{\partial}{\partial \alpha}  \log
      p_\alpha(\bx)   \\ =& E\left[ g(\bx) \frac{\partial}{\partial \alpha} \log
      p_\alpha(\bx) \right]. \end{align*} To achieve this, we used the
      derivative of the logarithm: \begin{gather*} y = \log u \\ \frac{\partial
      y}{\partial u} = \frac{1}{u} \frac{\partial u}{\partial x} \end{gather*}
      to write $$\frac{\partial}{\partial \alpha} p_\alpha(\bx) = p_\alpha(\bx)
      \frac{\partial}{\partial \alpha} \log p_\alpha(\bx).$$ This suggests a
      simple Monte Carlo algorithm for estimating the policy gradient: draw $N$
      random samples $\bx_i$ then estimate the gradient as $$
      \frac{\partial}{\partial \alpha} E \left[ g(\bx) \right] \approx
      \frac{1}{N} \sum_i g(\bx_i) \frac{\partial}{\partial \alpha} \log
      p_\alpha(\bx_i).$$</p>

      <p>This trick is even more potent in the optimal control case.  For a
      finite-horizon problem we have \begin{align*} \frac{\partial}{\partial
      \alpha} E \left[ \sum_{n=0}^N g(\bx[n], \bu[n]) \right] =& \sum_{n=0}^N
      \int d\bx[n] d\bu[n] ~ g(\bx[n],\bu[n]) \frac{\partial}{\partial \alpha}
      p_\alpha(\bx[n],\bu[n]) \\ =& E\left[ \sum_{n=0}^N g(\bx[n],\bu[n])
      \frac{\partial}{\partial \alpha} \log p_\alpha(\bx[n],\bu[n]) \right],
      \end{align*} where $$ p_\alpha(\bx[n],\bu[n]) = p_0(\bx[0]) \left(
      \prod_{k=1}^n p(\bx[k] | \bx[k-1],\bu[k-1]) \right) \left(\prod_{k=0}^n
      p_\alpha(\bu[k] | \bx[k]) \right).$$ Taking the $\log$ we have $$\log
      p_\alpha(\bx[n],\bu[n]) = \log p_0(\bx[0]) + \sum_{k=1}^n \log p(\bx[k] |
      \bx[k-1],\bu[k-1]) + \sum_{k=0}^n \log p_\alpha(\bu[k] | \bx[k]).$$  Only
      the last terms depend on $\alpha$, which yields $$\frac{\partial}{\partial
      \alpha} E \left[ \sum_{n=0}^N g(\bx[n], \bu[n]) \right] = E\left[
      \sum_{n=0}^N g(\bx[n],\bu[n]) \sum_{k=0}^n \frac{\partial}{\partial
      \alpha} \log p_\alpha(\bu[k]|\bx[k]) \right]. $$</p>

      <p>This update should surprise you.  It says that I can find the gradient
      of the long-term cost by taking only the gradient of the policy... but not
      the gradient of the plant, etc!  The intuition is that one can obtain the
      gradient by evaluating the policy along a number of (random) trajectory
      roll-outs of the closed-loop system, evaluating the cost on each, then
      increasing the probability in the policy of taking the actions correlated
      with lower long-term costs.</p>

      <p>This derivation is often presented as <b>the</b> policy gradient
      derivation.  The identity is certainly correct, but I'd prefer that you
      think of this as just one way to obtain the policy gradient. It's a
      particularly clever one in that it uses exactly the information that we
      have available in reinforcement learning -- we have access to the
      instantaneous costs, $g(\bx[n], \bu[n])$, and to the policy -- so are
      allowed to take gradients of the policy -- but does not require any
      understanding whatsoever of the plant model.   Although it's clever, it is
      not particularly efficient -- the Monte Carlo approximations of the
      expected value have a high variance, so many samples are required for an
      accurate estimate.  Other derivations are possible, some even simpler, and
      others that <b>do</b> make use of the plant gradients if you have access
      to them, and these will perform differently in the finite-sample
      approximations.</p>

      <p>For the remainder of this section, I'd like to dig in and try to
      understand the nature of the stochastic update a little better, to help
      you understand what I mean. </p>

    </section>

    <section data-type="sect2"><h1>Sample efficiency</h1>

      <p>Let's take a step back and think more generally about how one can use
      gradient descent in black-box (unconstrained) optimization. Imagine you
      have a simple optimization problem: $$\min_\alpha g(\alpha),$$ and you can
      directly evaluate $g()$, but not $\frac{\partial g}{\partial \alpha}$.
      How can you perform gradient descent?</p>

      <p>A standard technique for estimating the gradients, in lieu of
      analytical gradient information, is the method of finite
      differences<elib>Press92</elib>. The finite differences approach to
      estimating the gradient involves making the same small perturbation,
      $\epsilon$ to the input parameters in every dimension independently, and
      using: $$\pd{g}{\alpha_i} \approx \frac{g(\balpha+{\bf \epsilon}_i) -
      g(\alpha)}{\epsilon},$$ where ${\bf \epsilon_i}$ is the column vector with
      $\epsilon$ in the $i$th row, and zeros everywhere else.  Finite difference
      methods can be computationally very expensive, requiring $n+1$ evaluations
      of the function for every gradient step, where $n$ is the length of the
      input vector.</p>

      <p>What if each function evaluation is expensive?  Perhaps it means
      picking up a physical robot and running it for 10 seconds for each
      evaluation of $g()$.  Suddenly there is a premium on trying to optimize
      the cost function with the fewest number of evaluations on $g()$.  This is
      the name of the game in reinforcement learning -- it's often called RL's
      <i>sample complexity</i>.  Can we perform gradient descent using less
      evaluations?</p>

    </section>

    <section data-type="sect2"><h1>Stochastic Gradient Descent</h1>

      <p>This leads us to the question of doing approximate gradient descent, or
      "stochastic" gradient descent.  Thinking of the cost landscape as a
      Lyapunov function&dagger;<sidenote>&dagger; the Lyapunov function $V
      (\alpha) = g(\alpha) - g(\alpha^*)$ is commonly used in
      convergence/stability analysis of optimization algorithms. <todo>cite e.g.
      Ben Recht?</todo></sidenote>, then any update that moves downhill on each
      step will eventually reach the optimal.  More generally, any update that
      moves downhill on average will eventually reach a minimum... and sometimes
      "stochastic" gradient descent updates that occassionally go uphill but go
      downhill on average can even have desirable properties like bouncing out
      of small local minima.  The figure below gives some graphical intuition;
      for a formal treatment of stochastic gradient methods, see e.g.
      <elib>Bertsikas99</elib>.</p>

      <figure>
        <img width="90%" data-src="figures/weight_perturbation_quadratic_a.jpg"/>
        <figcaption>Stochastic gradient descent of a quadratic cost function .</figcaption>
      </figure>

    </section>

    <section data-type="sect2"><h1>The Weight Pertubation Algorithm</h1>

      <p>Instead of sampling each dimension independently, consider making a
      single small random change, $\bbeta$, to the parameter vector, $\balpha$.
      Now consider the update of the form: $$\Delta \balpha= -\eta [ g(\balpha +
      \bbeta) - g(\balpha)]\bbeta.$$  The intuition here is that if
      $g(\balpha+\bbeta) < g(\balpha)$, then $\bbeta$ was a good change and
      we'll move in the direction of $\bbeta$.  If the cost increased, then
      we'll move in the opposite direction.  Assuming the function is smooth and
      $\bbeta$ is small, then we will always move downhill on the Lyapunov
      function.</p>

      <p>Even more interesting, on average, the update is actually in the
      direction of the true gradient.  To see this, again we can assume the
      function is locally smooth and $\bbeta$ is small to write the Taylor
      expansion: $$g(\balpha + \bbeta) \approx g(\balpha) +
      \pd{g}{\balpha}\bbeta.$$  Now we have \begin{align*}\Delta \balpha
      \approx& -\eta \left[ \pd{g}{\alpha} \bbeta \right] \bbeta = -\eta \bbeta
      \bbeta^T \pd{g}{\balpha}^T \\ \avg{\Delta \balpha} \approx& -\eta
      \avg{\bbeta \bbeta^T} \pd{g}{\balpha}^T \end{align*} If we select each
      element of $\bbeta$ independently from a distribution with zero mean and
      variance $\sigma_\beta^2$, or $\avg{ \beta_i } = 0, \avg{ \beta_i \beta_j
      } = \sigma_\beta^2 \delta_{ij}$, then we have $$\avg{\Delta \balpha}
      \approx -\eta \sigma_\beta^2 \pd{g}{\balpha}^T.$$ Note that the
      distribution $p_{\alpha}(\bx)$ need not be Gaussian, but it is the
      variance of the distribution which determines the scaling on the
      gradient.</p>

    </section>

    <section data-type="sect2"><h1>Weight Perturbation with an Estimated
      Baseline</h1>

      <p>The weight perturbation update above requires us to evaluate the
      function $g$ twice for every update of the parameters.  This is low
      compared to the method of finite differences, but let us see if we can do
      better.  What if, instead of evaluating the function twice per update
      [$g(\balpha+\bbeta)$ and $g(\balpha)$], we replace the evaluation of
      $g(\balpha)$ with an estimator, $b = \hat{g}(\balpha)$, obtained from
      previous trials?  In other words, consider an update of the form:
      \begin{equation} \Delta \balpha= - \frac{\eta}{\sigma_\beta^2}
      [ g(\balpha + \bbeta) - b]\bbeta
      \label{eq:weight_perturbation_baseline} \end{equation} The estimator can
      take any of a number of forms, but perhaps the simplest is based on the
      update (after the $n$th trial): $$b[n+1] = \gamma g[n] + (1 -
      \gamma)b[n],\quad b[0] = 0, 0\le\gamma\le1,$$ where $\gamma$ parameterizes
      the moving average.  Let's compute the expected value of the update, using
      the same Taylor expansion that we used above: \begin{align*} E[\Delta
      \balpha] =& -\frac{\eta}{\sigma_\beta^2}  \avg{ [g(\balpha) +
      \pd{g}{\balpha}\bbeta - b]\bbeta} \\ =& -\frac{\eta}{\sigma_\beta^2}
      \avg{[g(\balpha)-b]\bbeta} -\frac{\eta}{\sigma_\beta^2} \avg{\bbeta
        \bbeta^T}\pd{g}{\balpha}^T \\
      =& - \eta \pd{g}{\balpha}^T \end{align*} In other words, the baseline does
      not effect our basic result - that the expected update is in the direction
      of the gradient.  Note that this computation works for any baseline
      estimator which is uncorrelated with $\bbeta$, as it should be if the
      estimator is a function of the performance in previous trials.</p>

      <p>Although using an estimated baseline doesn't effect the average update,
      it can have a dramatic effect on the performance of the algorithm.
        As we will see in a later section, if the evaluation of $g$ is
      stochastic, then the update with a baseline estimator can actually
      outperform an update using straight function evaluations.</p>

      <todo>
  % Note: it would be very interesting to look at the performance of
  % estimated baselines vs. perfect baselines (one has higher variance,
  % but allows 2x the updates for the same number of function evaluations).
      </todo>

      <p>Let's take the extreme case of $b=0$.  This seems like a very bad
      idea... on each step we will move in the direction of every random
      perturbation, but we will move more or less in that direction based on the
      evaluated cost. In average, we will still move in the direction of the
      true gradient, but only because we will eventually move more downhill than
      we move uphill.  It feels very naive.</p>

    </section>

    <section data-type="sect2"><h1>REINFORCE w/ additive Gaussian noise</h1>

      <p>Now let's consider the simple form of the REINFORCE update:
      \begin{align*} \frac{\partial}{\partial \alpha} E \left[ g(\bx) \right] =
      E\left[ g(\bx) \frac{\partial}{\partial \alpha} \log p_\alpha(\bx)
      \right]. \end{align*}  It turns out that weight perturbation is a
      REINFORCE algorithm.  To see this, take $\bx = \balpha + \bbeta$, $\bbeta
      \in N(0,\sigma^2)$, to have \begin{gather*} p_{\balpha}(\bx) =
      \frac{1}{(2\pi\sigma^2)^N} e^{\frac{-(\bx-\balpha)^T(\bx -
      \balpha)}{2\sigma^2}} \\ \log p_{\balpha}(\bx) =
      \frac{-(\bx-\balpha)^T(\bx - \balpha)}{2\sigma^2} + \text{terms that don't
      depend on }\balpha \\ \pd{}{\balpha} \ln p_{\balpha}(\bx) =
      \frac{1}{\sigma^2} (\balpha - \bx)^T = \frac{1}{\sigma^2} \bbeta^T. \\
      \end{gather*} If we use only a single trial for each Monte-Carlo
      evaluation, then the REINFORCE update is $$\Delta \balpha =
      -\frac{\eta}{\sigma^2} g(\balpha + \bbeta) \bbeta,$$ which is precisely
      the weight perturbation update (the crazy version with $b=0$ discussed
      above).  Although it will move in the direction of the gradient on
      average, it can be highly inefficient.</p>

    </section>

    <section data-type="sect2"><h1>Summary</h1>

      <p>The policy gradient "trick" from REINFORCE using log probabilities
      provides one means of estimating the true policy gradient.  It is not the
      only way to obtain the policy gradient... in fact the trivial
      weight-perturbation update obtains the same gradient for the case of a
      policy with a mean linear in $\alpha$ and a fixed diagonal covariance.
      It's cleverness lies in the fact that it makes use of the information we
      do have (instantaneous cost values and the gradients of the policy), but
      it's inefficiency comes from having a potentially very high variance.
      Variance reduction for policy gradient, often through baseline estimation,
      continues to be an active area of research.</p>

    </section>

  </section>

  <todo>REINFORCE for LTI systems</todo>


  <section data-type="sect1"><h1>Sample performance via the signal-to-noise
  ratio.</h1>

    <section data-type="sect2"><h1>Performance of Weight Perturbation</h1>

      <p>The simplicity of the REINFORCE / weight perturbation updates makes it
      tempting to apply them to problems of arbitrary complexity.  But a major
      concern for the algorithm is its performance - although we have shown that
      the update is in the direction of the true gradient <i>on average</i>, it
      may still require a prohibitive number of computations to obtain a local
      minima.</p>

      <p>In this section, we will investigate the performance of the weight
      perturbation algorithm by investigating its <i>signal-to-noise</i> ratio
      (SNR).   This idea was explored in <elib>Roberts09a</elib> -- my goal is
      just to give you a taste here. The SNR is the ratio of the power in the
      signal (here desired update in the direction of the true gradient) and the
      power in the noise (the remaining component of the update, so that
      $\Delta\balpha = -\eta\pd{g}{\balpha}^T + $
      noise)&dagger;<sidenote>&dagger; SNR could alternatively be defined as the
      ratio of the power of the component of the update in the direction of the
      signal and the component orthogonal to the signal (does not penalize the
      magnitudes of the updates): $$\frac{E[a^2]}{E\left[\left|\Delta\balpha -
      a{\bf v} \right|^2\right]},$$ where ${\bf v} =
      \frac{\pd{g}{\balpha}^T}{\left|\pd{g}{\balpha}^T\right|},$ $a = \Delta
      \balpha \cdot {\bf v}$.  This form is probably a better definition, but is
      more difficult to work with.</sidenote> $$\text{SNR} = \frac{\left|-\eta
      \pd{g}{\balpha}^T\right|^2}{\avg{\left|\Delta \balpha + \eta
      \pd{g}{\balpha}^T \right|^2}}.$$ In the special case of the unbiased
      update, the equation reduces to: $$\text{SNR} =
      \frac{\avg{\Delta\balpha}^T\avg{\Delta\balpha}}
      {\avg{(\Delta\balpha)^T(\Delta\balpha)} -
      \avg{\Delta\balpha}^T\avg{\Delta\balpha}}.$$</p>

      <p>For the weight perturbation update we have: $$\avg{\Delta
      \balpha}^T\avg{\Delta \balpha} = \eta^2 \pd{g}{\balpha} \pd{g}{\balpha}^T
      = \eta^2 \sum_{i=1}^{N} \left( \pd{g}{\alpha_i} \right)^2,$$
      \begin{align*} \avg{(\Delta \balpha)^T(\Delta \balpha)} =&
      \frac{\eta^2}{\sigma_\beta^4} \avg{\left[g(\balpha+\bbeta) -
      g(\balpha)\right]^2 \bbeta^T\bbeta} \\ \approx&
      \frac{\eta^2}{\sigma_\beta^4} \avg{ \left[ \pd{g}{\balpha} \bbeta
      \right]^2 \bbeta^T\bbeta } \\ =& \frac{\eta^2}{\sigma_\beta^4} \avg{
      \left(\sum_i \pd{g}{\alpha_i}\beta_i\right) \left( \sum_j \pd{g}{\alpha_j}
      \beta_j\right) \left(\sum_k \beta_k^2 \right) } \\ =&
      \frac{\eta^2}{\sigma_\beta^4} \avg{ \sum_i \pd{g}{\alpha_i}\beta_i \sum_j
      \pd{g}{\alpha_j} \beta_j \sum_k \beta_k^2 } \\ =&
      \frac{\eta^2}{\sigma_\beta^4} \sum_{i,j,k} \pd{g}{\alpha_i}
      \pd{g}{\alpha_j} \avg{ \beta_i \beta_j \beta_k^2 }\\ \avg{\beta_i \beta_j
      \beta_k^2} = \begin{cases} 0 & i \neq j \\ \sigma_\beta^4 & i=j\neq k \\
      \mu_4(\beta) & i = j =k \end{cases}& \\ =& \eta^2 (N-1) \sum_i \left(
      \pd{g}{\alpha_i} \right)^2 +  \frac{\eta^2 \mu_4(z)}{\sigma_\beta^4}
      \sum_i \left(\pd{g}{\alpha_i}\right)^2 \end{align*} where $\mu_n(z)$ is
      the $n$th central moment of $z$: $$\mu_n(z) = \avg{(z - \avg{z})^n}.$$
      Putting it all together, we have: $$\text{SNR} = \frac{1}{N - 2 +
      \frac{\mu_4(\beta_i)}{\sigma_\beta^4}}.$$</p>

      <div data-type="example"><h1>Signal-to-noise ratio for additive Gaussian
        noise</h1>

        <p>For $\beta_i$ drawn from a Gaussian distribution, we have $\mu_1 = 0,
        \mu_2 = \sigma^2_\beta, \mu_3 = 0, \mu_4 = 3 \sigma^4_\beta,$
        simplifying the above expression to: $$\text{SNR} = \frac{1}{N+1}.$$</p>

      </div>

      <div data-type="example"><h1>Signal-to-noise ratio for additive uniform
        noise</h1>

        <p>For $\beta_i$ drawn from a uniform distribution over [-a,a], we have
        $\mu_1 = 0, \mu_2 = \frac{a^2}{3} = \sigma_\beta^2, \mu_3 = 0, \mu_4 =
        \frac{a^4}{5} = \frac{9}{5}\sigma_\beta^4,$ simplifying the above to:
        $$\text{SNR} = \frac{1}{N-\frac{1}{5}}.$$</p>

      </div>

      <p>Performance calculations using the SNR can be used to design the
      parameters of the algorithm in practice.  For example, based on these
      results it is apparent that noise added through a uniform distribution
      yields better gradient estimates than the Gaussian noise case for very
      small $N$, but that these differences are neglibile for large $N$.</p>

      <p>Similar calculations can yield insight into picking the size of the
      additive noise (scaled by $\sigma_\beta$).  The calculations in this
      section seem to imply that larger $\sigma_\beta$ can only reduce the
      variance, overcoming errors or noise in the baseline estimator,
      $\tilde{b}$; this is a short-coming of our first-order Taylor expansion.
      If the cost function is not linear in the parameters, then an examination
      of higher-order terms reveals that large $\sigma_\beta$ can increase the
      SNR.  The derivation with a second-order Taylor expansion is left for an
      exercise.</p>

    </section>

  </section>

  <todo>Better baselines with Importance sampling</todo>

  <!--
  \begin{exercises}


  \begin{matlabex}[Weight perturbation on a quadratic cost]
  \label{ex:weight_perturbation_quadratic}
  In this problem we will optimize a cost quadratic cost function using
  weight perturbation.
  $$y(\balpha) = \frac{1}{2} \balpha^T {\bf A} \balpha\text{, with } {\bf A} = {\bf A}^T$$
  \begin{subex}
  Simulate the weight perturbation algorithm for ${\bf A} = {\bf I}_{2\times2}$.
  Plot the values of $\balpha$ at each iteration of the algorithm on top of
  the cost function, $y$.  Your results should resemble
  figure \ref{f:weight_perturbation_quadratic}, which was made with
  these parameters.  Try using additive Gaussian noise, and additive
  uniform noise.  How does the performance of convergence depend on
  $\eta$? on $\sigma_\beta^2$?
  \end{subex}
  \begin{subex}[Signal-to-noise ratio]
  Estimate the signal-to-noise ratio of your weight pertubation update
  by holding $\balpha$ fixed and computing $\Delta \balpha$ for as many trials
  as are required for your SNR statistics to converge.  Using ${\bf A} =
  {\bf I}_{N \times N}$, make a plot of the SNR for Gaussian and uniform
  additive noise as a function of $N$.  Compare these plots with the
  theoretical predictions.
  \end{subex}

  \begin{solution}
  \begin{itemize}
  \item[(a)]
  \verbatiminput{figures//weight_perturbation_quadratic_a.m}
  \item[(b)]
  \begin{figure}[!h]
  \centerline{\includegraphics[width=4in]{figures/weight_perturbation_quadratic_b.jpg}}
  \caption{Signal-to-noise ratio calculations}
  \label{f:weight_perturbation_quadratic_b}
  \end{figure}
  The results are plotted in figure \ref{f:weight_perturbation_quadratic_b}
  \verbatiminput{figures/weight_perturbation_quadratic_b.m}
  \end{itemize}
  \end{solution}
  \end{matlabex}

  \begin{ex}[Weight perturbation performance with a baseline estimator]
  \label{p:snr_baseline}

  \begin{solution}
  Using $\tilde{b} = y(\balpha) - b$, we have:
  $$\avg{\Delta \balpha}^T\avg{\Delta \balpha} = \eta^2 \pd{J}{\balpha} \pd{J}{\balpha}^T = \eta^2
  \sum_{i=1}^{N} \left( \pd{J}{\alpha_i} \right)^2,$$
  \begin{align*}
  \avg{(\Delta \balpha)^T(\Delta \balpha)} =& \frac{\eta^2}{\sigma_\beta^4}
  \avg{\left[y(\balpha+\bbeta) - y(\balpha)\right]^2 \bbeta^T\bbeta} \\
  \approx& \frac{\eta^2}{\sigma_\beta^4} \avg{ \left[ \tilde{b} + \pd{J}{\balpha} \bbeta \right]^2 \bbeta^T\bbeta } \\
  =& \frac{\eta^2}{\sigma_\beta^2} \avg{\tilde{b}^2} +
  2 \frac{\eta^2}{\sigma_\beta^4} \avg{ \tilde{b} \left(\sum_i \pd{J}{\alpha_i}\beta_i\right)
  \left(\sum_j \beta_j^2 \right) } + \text{ previous
  terms} \\
  =& \frac{\eta^2}{\sigma_\beta^2} \avg{\tilde{b}^2} +
  2 \frac{\eta^2}{\sigma_\beta^4} \sum_{i,j} \pd{J}{\alpha_i} \avg{ \beta_i \beta_j^2 }
  + \text{ previous
  terms} \\
  =& \frac{\eta^2}{\sigma_\beta^2}\sigma^2_{\tilde{b}} +
  2 \frac{\eta^2}{\sigma^4} \sum_i \pd{J}{\alpha_i} \mu_3(\beta_i) + \eta^2 (N-1)
  \sum_i \left( \pd{J}{\alpha_i} \right)^2 +  \frac{\eta^2}{\sigma_\beta^4}
  \sum_i \left(\pd{J}{\alpha_i}\right)^2 \mu_4(\beta_i)
  \end{align*}
  For Gaussian (and all distributions which are symmetric about their
  mean), we have $\mu_3(\beta_i)=0$, and therefore: $$\text{SNR} =
  \frac{1}{N+1+n_b},$$ where $n_b$ is the contribution from errors in
  the baseline estimate $b$: $$n_b =
  \frac{\sigma^2_{\tilde{b}}}{\sigma_\beta^2 \sum_i
  \left( \pd{J}{\alpha_i} \right)^2}.$$ For uniform, we have $$\text{SNR} =
  \frac{1}{N-\frac{1}{5} +n_b}.$$
  \end{solution}
  \end{ex}

  \begin{ex}[Weight perturbation - optimal noise calculations]
  \label{p:optimal_noise}
  Use a second-order Taylor expansion...
  $$y(\balpha + \bbeta) = y(\balpha) + \pd{J}{\balpha} \bbeta + \frac{1}{2}\bbeta^T {\bf h} \bbeta,$$
  where ${\bf h}$ is the Hermitian of the function evaluated at $\balpha$:
  $${\bf h} = \pd{}{\balpha} \left[ \pd{J}{\balpha}^T \right] = \begin{bmatrix}
  \pd{^2 y}{\alpha_1 \partial \alpha_1} & \cdots & \pd{^2 y}{\alpha_1 \partial \alpha_N} \\
  \vdots & \ddots & \vdots \\ \pd{^2 y}{\alpha_N \partial \alpha_1} & \cdots &
  \pd{^2 y}{\alpha_N \partial \alpha_N} \end{bmatrix}.$$
  You may assume that the distribution on $\beta_i$ is symmetric about the
  mean, which zeros all odd central-moments starting at $\mu_3(\beta_i) = 0$.

  \begin{solution}
  \begin{align*}
  \avg{\Delta\balpha} \approx& -\frac{\eta}{\sigma_\beta^2} \avg{ \left[ \tilde{b} +
  \pd{J}{\balpha}\bbeta + \frac{1}{2} \bbeta^T {\bf h} \bbeta \right] \bbeta } \\
  =& - \eta \pd{J}{\balpha}^T + \sum_i \mu_3(\beta_i) h_{ii} = -\eta \pd{J}{\balpha}^T
  \end{align*}
  \begin{align*}
  \avg{\Delta \balpha^T \Delta \balpha} \approx& \frac{\eta^2}{\sigma_\beta^4}
  \avg{ \left[ \tilde{b} + \pd{J}{\balpha}\bbeta + \frac{1}{2}\bbeta^T {\bf h} \bbeta \right]^2 \bbeta^T
  \bbeta }
  \end{align*}
  This introduces a number of new terms to the soln from problem
  \ref{p:snr_baseline}:
  \begin{align*}
  \tilde{b} ( \bbeta^T {\bf h} \bbeta ) \bbeta^T \bbeta =&
  \tilde{b} \sum_{i,j,k} h_{ij} \avg{\beta_i \beta_j \beta_k^2} \\
  =& \tilde{b} \left[ \sigma_\beta^4 (N-1) \sum_i h_{ii} + \sum_i h_{ii}
  \mu_4(\beta_i) \right]
  \end{align*}
  \begin{align*}
  (\pd{J}{\balpha} \bbeta ) (\bbeta^T {\bf h} \bbeta) ) \bbeta^T \bbeta =&
  \sum_{i,j,k,l} \pd{J}{\alpha_i} h_{jk} \avg{\beta_i \beta_j \beta_k \beta_l^2} = 0
  \end{align*}
  because $$ \avg{\beta_i \beta_j \beta_k \beta_l^2} = \begin{cases} 0 & i \neq j, \text{ or } i
  \neq k, \text{ or } j \neq k
  \\ \mu_3(\beta_i) \mu_2(\beta_i) & i=j=k \neq l \\
  \mu_5(\beta_i) & i=j=k=l. \end{cases}$$
  \begin{align*}
  \frac{1}{4}(\bbeta^T {\bf h} \bbeta) (\bbeta^T {\bf h} \bbeta)
  \bbeta^T \bbeta =&
  \sum_{i,j,k,l,m} h_{ij} h_{kl} \avg{\beta_i \beta_j \beta_k \beta_l \beta_m^2} \\ =&
  \sigma_\beta^6 (N-2) \sum_{i,j \neq i} (h_{ii}h_{jj} + 2h_{ij}^2) +
  \sigma_\beta^2 \mu_4(\beta_i) \left[ (N-1)\sum_i h_{ii}^2 + \sum_{i,j \neq i}  (2h_{ii}h_{jj}
  + 2h_{ij}^2) \right]
  \end{align*}
  because $$\avg{\beta_i \beta_j \beta_k \beta_l \beta_m^2} = \begin{cases} \sigma_\beta^6 &
  \text{three distinct pairs} \\ \sigma_\beta^2 \mu_4(\beta_i) & \text{two
  pairs} \\ 0 & \text{otherwise}.\end{cases}$$
  yuck!
  \end{solution}
  \end{ex}

  \end{exercises}
  -->

  <todo>Covariance Matrix Adaptation</todo>
  <todo>Policy Improvement with Path Integrals</todo>

</section>

<section data-type="chapter" class="chapter" id="value_learning"><h1>Model
  -Free Value Function Methods</h1>


</section>

<section data-type="chapter" class="chapter"
         id="actor_critic"><h1>Actor-Critic Methods</h1></section>

</section>


</div> <!-- end of part III -->

<appendix class="part" title="Appendix">

<section data-type="chapter" class="chapter" id="drake"><h1>Drake</h1>

  <p>The <drake></drake> website is the main source for information and
  documentation.  This goal of this chapter provide a few specific pointers
  and recommended workflows to get you up and running quickly with the
  examples provided in these notes.</p>


  <p><b>NOTE: If you are taking the Spring 2018 class, we have provided a Docker
  workflow for you to make sure you are kept up to date week by week.  Do NOT
  use the instructions below.  <a
  href="http://underactuated.csail.mit.edu/Spring2018/install_drake_docker.html">Follow
  these instructions instead.</a></b></p>

  <div style="height:30px"></div>

  <p>First, pick your platform (click on your OS):</p>

  <p style="text-align:center">
  <span class="linux"><b>Ubuntu Linux</b>  |
  <span onclick="setPlatform(true);" style="text-decoration:underline;
  cursor:pointer">Mac Homebrew</span></span><span class="mac"> <span
  onclick="setPlatform(false);" style="text-decoration:underline;
  cursor:pointer">Ubuntu Linux</span> | <b>Mac Homebrew</b> </span></p>

  <section data-type="sect1"><h1>Download the textbook supplement</h1>

    <div><pre class="highlight"><code>git clone https://github.com/RussTedrake/underactuated.git</code></pre></div>

    <p>and install the prerequisites using the platform-specific installation script provided:</p>

    <div class="linux"><pre class="highlight"><code>sudo underactuated/scripts/setup/ubuntu/16.04/install_prereqs
export PYTHONPATH=`pwd`/underactuated/src:${PYTHONPATH}</code></pre></div>

    <div class="mac"><pre class="highlight"><code>underactuated/scripts/setup/mac/install_prereqs
export PYTHONPATH=`pwd`/underactuated/src:${PYTHONPATH}</code></pre></div>

  </section>

  <section data-type="sect1"><h1>Install Drake</h1>

    <p>Although I hope that power users will build <drake></drake> from source
    (and contribute back), I recommend using the precompiled binaries to get
    started.  The links below indicate the specific distributions that the
    examples in this text have been tested against.</p>

    <section data-type="sect2"><h1>Download the binaries</h1>

      <div class="linux"><pre class="highlight"><code>curl -o drake.tar.gz <div id="drake-linux-binaries" style="display:inline">https://drake-packages.csail.mit.edu/drake/nightly/drake-latest-xenial.tar.gz</div></code></pre></div>

      <div class="mac"><pre class="highlight"><code>curl -o drake.tar.gz <div id="drake-mac-binaries" style="display:inline">https://drake-packages.csail.mit.edu/drake/nightly/drake-latest-mac.tar.gz</div></code></pre></div>

    </section>

    <section data-type="sect2"><h1>Unpack and set your PYTHONPATH</h1>

      <div class="linux"><pre class="highlight"><code>sudo tar -xvzf drake.tar.gz -C /opt
sudo /opt/drake/share/drake/setup/install_prereqs
export PYTHONPATH=/opt/drake/lib/python2.7/site-packages:${PYTHONPATH}</code></pre></div>

      <div class="mac"><pre class="highlight"><code>sudo tar -xvzf drake.tar.gz -C /opt
/opt/drake/share/drake/setup/install_prereqs
export PYTHONPATH=/opt/drake/lib/python2.7/site-packages:${PYTHONPATH}
export PATH=/usr/local/opt/python/libexec/bin:${PATH}</code></pre></div>
    </section>

    <section data-type="sect2"><h1>Test for success</h1>

      <p><pre class="highlight"><code>python -c 'import pydrake; print(pydrake.__file__)'
</code></pre></p>

      <p> Further tips and instructions are available at the <a
      href="http://drake.mit.edu/python_bindings.html#using-the-python-bindings">python
      documentation page on the drake website</a>. </p>

    </section>

  </section>

  <section data-type="sect1"><h1>Run an example</h1>

    <p>Note: All of the examples in the textbook supplement currently assume
    that working directory of your python interpreter is the
    <code>underactuated/src</code> directory.</p>

      <pysrc>double_pendulum/simulator.py</pysrc>

  </section>

  <section data-type="sect1"><h1>Consider Jupyter Notebook</h1>

    <p>The examples you'll find throughout the text are provided so that
    they can be run in any python interpreter.  However, many people
    <i>love</i> the <a href="http://jupyter.org/">Jupyter Notebook</a>
    workflow, and the code should work for you there, as well.</p>

    <p><a href="src/double_pendulum/simulator.ipynb">Here is an example</a> that
    shows you how to tell <code>matplotlib</code> to perform well in the
    notebook (including with animations).</p>

    <div><pre class="highlight"><code>cd underactuated/src/double_pendulum
jupyter notebook simulator.ipynb</code></pre></div>

  </section>

  <div style="height:30px"></div>

  <todo>
    Docker?
    
    Troubleshooting.
  </todo>

  <p>The next chapter in the Appendix provides a tutorial for working with
  dynamical systems in <drake></drake>.</p>

  <section data-type="sect1"><h1>Listing of <drake></drake> examples throughout this text</h1>

    <p>The best overview we have for the remaining capabilities in <drake></drake> are the main chapters of these notes.  For convenience, here is a list of software examples that are sprinkled throughout the text:</p>

    <div id="list_of_drake_examples"><center>Coming soon!</center></div>

  </section>

</section> <!-- end chapter on drake -->

<section data-type="chapter" class="chapter" id="systems"><h1>Modeling
  Input-Output Dynamical Systems in Drake</h1>

  <p>This chapter provides a short tutorial for modeling input-output dynamical
  systems in <drake></drake>. <drake></drake> includes a set of C++ classes in
  <code>drake::systems</code> that provide an API for building complex dynamical
  systems by connecting simple dynamical systems into a block diagram.  The
  modeling approach is very much inspired by MATLAB's Simulink (though without
  the graphical user interface).  Like Simulink, <code>drake::systems</code>
  provides a library of existing systems and also makes it easy for you to
  define your own new dynamical systems.  The python wrappers in
  <code>pydrake</code> used in these notes provide a convenient set of
  bindings on top of this systems framework, allowing you to write new
  systems in either Python or C++, and to use them interchangeably.</p>

  <p>A major difference between modeling dynamical systems in
  <drake></drake> vs Simulink (or most other system modeling
  languages) is that there is a considerable emphasis in <drake></drake> on
  exposing the underlying structure in the governing equations.  For example, if
  certain outputs of a dynamical system depend on only a small fraction of the
  inputs, then we attempt to expose this sparsity directly.  Similarly, if the
  dynamics of a system can be described by a polynomial vector field (or linear,
  or at least differentiable, ...), then <drake></drake> aims to expose this
  structure as well, and to preserve this structure when systems are combined
  into a diagram. Exposing this structure may have only modest benefit for
  numerical simulation, but it is incredibly enabling for more sophisticated
  algorithms for analysis, system identification, and feedback or state
  estimator design. </p>

  <p>We'll return to the more advanced modeling tools in a minute, but let us
  start with some very simple examples.</p>

  <p>A quick note on documentation.  The C++ classes are beautifully documented
  in <a href="http://drake.mit.edu/doxygen_cxx">doxygen</a>.  I <a
  href="https://github.com/RobotLocomotion/drake/issues/7915">currently
  recommend</a> using this doxygen as the primary source of documentation even
  for your Python development... most of the <code>pydrake</code> API maps
  directly to that C++ API.</p>

  <section data-type="sect1"><h1>Writing your own dynamics</h1>

    <p> In this section, we will describe how you can write your own dynamics
    class in pydrake.  Every user system should derive from the
    <code>pydrake.systems.framework.LeafSystem</code> class.</p>

    <p> However, many dynamical systems we are interested in are represented by
    a simple vector field in state-space form, where we often use $x$ to denote the
    state vector, $u$ for the input vector, and $y$ for the output vector.  To
    make it even easier to implement systems in this form, we provide another
    subclass <code>pydrake.systems.framework.VectorSystem</code> that provides
    an excellent starting point. </p>

    <center><img style="width:250px;" data-src="figures/input_output_system.svg"/></center>

    <p> <b>Continuous-Time Systems.</b> Consider a basic continuous-time
    nonlinear input-output dynamical system described by the following
    state-space equations: \begin{gather*} \dot{x} = f(t,x,u), \\ y = g(t,x,u).
    \end{gather*} In <code>pydrake</code>, you can instantiate a system of this
    form where $f()$ and $g()$ are anything that you can write into a Python
    function, as illustrated by the following example:

    <div data-type="example" class="drake"><h1>A simple continuous-time system</h1>

      <p>Consider the system \begin{gather*}\dot{x} = -x + x^3,\\ y =
      x.\end{gather*}  This system has zero inputs, one (continuous) state
      variable, and one output.  It can be implemented in <drake></drake> using
      the following code:</p>

      <pysrcinclude>simple/continuous_time_system.py</pysrcinclude>

    </div>


    <p><b>Discrete-Time Systems</b>. Implementing a basic discrete-time system
    in <drake></drake> is very analogous to implementing a continuous-time
    system.  The discrete-time system given by: \begin{gather*} x[n+1] =
    f(n,x,u),\\ y[n] = g(n,x,u), \end{gather*} can be implemented as seen in the
    following example.</p>

    <div data-type="example" class="drake"><h1>A simple discrete-time
    system</h1>

      <p>Consider the system \begin{gather*}x[n+1] = x^3[n],\\ y[n] =
      x[n].\end{gather*}  This system has zero inputs, one (discrete) state
      variable, and one output.  It can be implemented in <drake></drake> using
      the following code:</p>

      <pysrcinclude>simple/discrete_time_system.py</pysrcinclude>

    </div>


    <p>Although these simple VectorSystems are a nice way to get started, in
    fact Drake supports authoring a wide variety of systems with multiple inputs
    and outputs, mixed discrete- and continuous- dynamics, hybrid dynamics with
    guards and resets, systems with constraints, and even stochastic systems.
    To expose more of the underlying system framework, you should derive your
    system from <a
    href="http://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_leaf_system.html">LeafSystem</a>
    directly, instead of using the simplified VectorSystem interface. </p>

    <todo>restore/update mixed discrete/continuous, systems with
      constraints, hybrid systems, and stochastic systems.  Also
      RigidBodyPlant (load from urdf/sdf).</todo>

  </section>

  <section data-type="sect1"><h1>Simulation</h1>

    <p>Once you have acquired a System object describing the dynamics of
    interest, the most basic thing that you can do is to simulate it.  This is
    accomplished with the <code>pydrake.framework.analysis.Simulator</code>
    class. This class provides access to a rich suite of numerical integration
    routines, with support for variable-step integration, stiff solvers, and
    event detection.</p>

    <p>In order to view the data from a simulation after the simulation has run,
    you should add a <code>pydrake.framework.primitives.SignalLogger</code>
    system to your diagram.</p>

    <div data-type="example" class="drake"><h1>Simulating and
    plotting the results</h1>

      <p>Use the following code to instantiate the
      <code>SimpleContinuousTimeSystem </code>, simulate it, and plot the
      results:</p>
      <pysrcinclude>simple/continuous_time_system_simulation.py</pysrcinclude>

      <p>This should generate the plot below</p>
      <figure>
        <img style="width:500px"
             data-src="figures/simple_continuous_time_system_simulation.svg"/>
        <figcaption>Simulation and plot the simple continuous time system
          .</figcaption>
      </figure>

      <p>The code for running the discrete time system is basically identical:</p>
      <pysrcinclude>simple/discrete_time_system_simulation.py</pysrcinclude>

    </div>

    <p>For many systems, the simulation will run much faster than real time.
    If you would like to tell the simulator to slow down (if possible) to some
    multiple of the real time clock, then consider using the
    <code>set_target_realtime_rate</code> method of the <code>Simulator</code>.
    This is useful, for example, if you are trying to animate your robot as it
    simulates, and would like the benefit of physical intuition, or if you are
    trying to use the simulation as a part of a multi-process real-time control
    system.</p>

  </section>

  <section data-type="sect1"><h1>The System "Context"</h1>

    <p>If you were looking carefully, you might have noticed a few instances of
    the word "context" in the code snippets above.  The <a
    href="http://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_context.html">Context</a>
    is a core concept in the <drake></drake> systems framework: the Context
    captures all of the (potentially) dynamic information that a System requires
    to implement it's core methods: this includes the time, the state, any
    inputs, and any system parameters.  The Context of a System is everything
    you need to know for simulation (or control design, ...), and given a
    Context all methods called on a System should be completely
    deterministic/repeatable.</p>

    <p>Systems know how to create an instance of a Context (see <a
    href="http://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_system.html#ab763e3e9fc29dfc3ccde21ca7501a7e9">CreateDefaultContext</a>).
    In the simulation example above, the Simulator created a Context for us. We
    retreived the Context from the Simulator in order to set the initial
    conditions (state) of the system before running the simulation.</p>

    <p> Note that a Context is not completely defined unless all of the input
    ports are connected (simulation and other method calls will fail if they are
    not). For input ports that are not directly tied to the output of anther
    system, consider using the <a
    href="http://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_context.html#ab6431f682a893a81eef20d675c996d8c">FixInputPort</a>
    method of the Context.</p>

  </section>

  <section data-type="sect1"><h1>Combinations of Systems: Diagram
  and DiagramBuilder</h1>

    <figure>
      <center><img style="width:250px;" data-src="figures/feedback_system.svg"/></center>
      <figcaption>Feedback combination</figcaption>
    </figure>
    <figure>
      <center><img style="width:300px;" data-src="figures/cascade_system.svg"/></center>
      <figcaption>Cascade combination</figcaption>
    </figure>

    <p>The real modeling power of <drake></drake> comes from combining many
    smaller systems together into more complex systems.  The concept is very
    simple: we use the <code>DiagramBuilder</code> class to <code>AddSystem
    </code>s and to <code>Connect</code> input ports to output ports or to
    expose them as inputs/output of the diagram.  Then we call <code>Build
    </code> to generate the new <code>Diagram</code> instance, which is just
    another <code>System</code> in the framework, and can be simulated or
    analyzed using the entire suite of tools.</p>

    <div data-type="example" class="drake"><h1>DiagramBuilder</h1>

      <p>In the example below, we connect two subsystems (the plant and the
      visualizer), and expose the input of the plant as an input to the
      <code>Diagram</code> being constructed:</p>

      <pysrcinclude>double_pendulum/simulator.py</pysrcinclude>

    </div>

    <p>Just like any other <code>System</code>, a <code>Diagram</code> has
    a <code>Context</code>.  You can always extract the context of a
    subsystem using <code>Diagram.GetSubSystemContext</code> or
    <code>Diagram.GetMutableSubsystemContext</code>.  (If it is not clear
    yet, "mutable" means you have write access, otherwise the data is
    <code>const</code>; we go to some length in the systems framework to
    protect algorithms / users from violating the fundamental systems
    abstractions).</p>

  </section>


</section> <!-- end chapter on modeling dynamical systems -->

<section data-type="chapter" class="chapter" id="multibody"><h1>Rigid-Body
  Dynamics</h1>

  <section data-type="sect1"><h1>Deriving the equations of motion (an example)</h1>

    <p>The equations of motion for a standard robot can be derived using the
    method of Lagrange.  Using $T$ as the total kinetic energy of the system,
    and $U$ as the total potential energy of the system, $L = T-U$, and $Q_i$ as
    the generalized force corresponding to $q_i$, the Lagrangian dynamic
    equations are: \begin{equation} \frac{d}{dt}\pd{L}{\dot{q}_i} - \pd{L}{q_i}
    = Q_i.\end{equation} If you are not comfortable with these equations, then
    any good book chapter on rigid body mechanics can bring you up to speed --
    try <elib>Craig89</elib> for a very practical guide to robot
    kinematics/dynamics, <elib>Goldstein02</elib> for a more hard-core dynamics
    text or <elib>Thornton03</elib> for a classical dynamics text which is a
    nice read -- for now you can take them as a handle that you can crank to
    generate equations of motion. </p>

    <div data-type="example"><h1>Simple Double Pendulum</h1>

      <figure>
        <img style="width:250px;" data-src="figures/simple_double_pend.svg"/>
        <figcaption>Simple double pendulum</figcaption>
      </figure>

      <p> Consider the simple double pendulum with torque actuation at both
      joints and all of the mass concentrated in two points (for simplicity).
      Using $\bq = [\theta_1,\theta_2]^T$, and ${\bf p}_1,{\bf p}_2$ to denote
      the locations of $m_1,m_2$, respectively, the kinematics of this system
      are:

      \begin{eqnarray*}
      {\bf p}_1 =& l_1\begin{bmatrix} s_1 \\ - c_1 \end{bmatrix}, &{\bf p}_2  =
      {\bf p}_1 + l_2\begin{bmatrix} s_{1+2} \\ - c_{1+2} \end{bmatrix} \\
      \dot{{\bf p}}_1 =& l_1 \dot{q}_1\begin{bmatrix} c_1 \\ s_1 \end{bmatrix},
      &\dot{{\bf p}}_2 = \dot{{\bf p}}_1 + l_2 (\dot{q}_1+\dot{q}_2) \begin{bmatrix} c_{1+2} \\ s_{1+2} \end{bmatrix}
      \end{eqnarray*}

      Note that $s_1$ is shorthand for $\sin(q_1)$, $c_{1+2}$ is shorthand for
      $\cos(q_1+q_2)$, etc. From this we can write the kinetic and potential
      energy:

      \begin{align*}
      T =& \frac{1}{2} m_1 \dot{\bf p}_1^T \dot{\bf p}_1 + \frac{1}{2} m_2
      \dot{\bf p}_2^T \dot{\bf p}_2 \\
      =& \frac{1}{2}(m_1 + m_2) l_1^2 \dot{q}_1^2 + \frac{1}{2} m_2 l_2^2 (\dot{q}_1 + \dot{q}_2)^2 + m_2 l_1 l_2 \dot{q}_1 (\dot{q}_1 + \dot{q}_2) c_2 \\
      U =& m_1 g y_1 + m_2 g y_2 = -(m_1+m_2) g l_1 c_1 - m_2 g l_2 c_{1+2}
      \end{align*}

      Taking the partial derivatives $\pd{T}{q_i}$, $\pd{T}{\dot{q}_i}$, and
      $\pd{U}{q_i}$ ($\pd{U}{\dot{q}_i}$ terms are always zero), then
      $\frac{d}{dt}\pd{T}{\dot{q}_i}$, and plugging them into the Lagrangian,
      reveals the equations of motion:

      \begin{align*}
      (m_1 + m_2) l_1^2 \ddot{q}_1& + m_2 l_2^2 (\ddot{q}_1 + \ddot{q}_2) + m_2 l_1 l_2 (2 \ddot{q}_1 + \ddot{q}_2) c_2 \\
      &- m_2 l_1 l_2 (2 \dot{q}_1 + \dot{q}_2) \dot{q}_2 s_2 + (m_1 + m_2) l_1 g s_1 + m_2 g l_2 s_{1+2} = \tau_1 \\
      m_2 l_2^2 (\ddot{q}_1 + \ddot{q}_2)& + m_2 l_1 l_2 \ddot{q}_1 c_2 + m_2 l_1 l_2
      \dot{q}_1^2 s_2 + m_2 g l_2 s_{1+2} = \tau_2
      \end{align*}

      As we saw in chapter 1, numerically integrating (and animating) these
      equations in <drake></drake> produces the expected result. </p>

    </div>

  </section>

  <section data-type="sect1"><h1>The Manipulator Equations</h1>

    <p> If you crank through the Lagrangian dynamics for a few simple robotic
    manipulators, you will begin to see a pattern emerge - the resulting
    equations of motion all have a characteristic form.  For example, the
    kinetic energy of your robot can always be written in the form:
    \begin{equation} T = \frac{1}{2} \dot{\bq}^T \bM(\bq)
    \dot{\bq},\end{equation} where $\bM$ is the state-dependent inertia matrix
    (aka mass matrix). This observation affords some insight into general
    manipulator dynamics - for example we know that ${\bf M}$ is always positive
    definite, and symmetric<elib>Asada86</elib>(p.107) and has a beautiful
    sparsity pattern<elib>Featherstone05</elib> that we'll attempt to take
    advantage of in our algorithms.</p>


    <p>Continuing our abstractions, we find that the equations of motion of a
    general robotic manipulator (without kinematic loops) take the form
    \begin{equation}{\bf M}(\bq)\ddot{\bq} + \bC(\bq,\dot{\bq})\dot{\bq} =
    \btau_g(\bq) + {\bf B}\bu, \label{eq:manip} \end{equation} where $\bq$ is
    the state vector, ${\bf M}$ is the inertia matrix, $\bC$ captures Coriolis
    forces, and $\btau_g$ is the gravity vector.  The matrix $\bB$ maps control
    inputs $\bu$ into generalized forces.  Note that we pair $\bM\ddot\bq +
    \bC\dot\bq$ on the left side because "... the equations of motion depend on
    the choice of coordinates $\bq$.  For this reason neither $\bM\ddot\bq$ nor
    $\bC\dot\bq$ individually should be thought of as a generalized force; only
    their sum is a force"<elib>Choset05</elib>(s.10.2).  Indeed, whenever I
    write Eq. (\ref{eq:manip}), I see $ma = F$. </p>

    <div data-type="example"><h1>Manipulator Equation form of the Simple Double
    Pendulum</h1> The equations of motion from Example 1 can be written
    compactly as: \begin{align*} \bM(\bq) =& \begin{bmatrix} (m_1 + m_2)l_1^2 +
    m_2 l_2^2 + 2 m_2 l_1l_2 c_2 & m_2 l_2^2 + m_2 l_1 l_2 c_2 \\ m_2 l_2^2 +
    m_2 l_1 l_2 c_2 & m_2 l_2^2 \end{bmatrix} \\ \bC(\bq,\dot\bq) =&
    \begin{bmatrix} 0 & -m_2 l_1 l_2 (2\dot{q}_1 + \dot{q}_2)s_2 \\ m_2 l_1 l_2
    \dot{q}_1 s_2 & 0 \end{bmatrix} \\ \btau_g(\bq) =& -g \begin{bmatrix} (m_1 +
    m_2) l_1 s_1 + m_2 l_2 s_{1+2} \\ m_2 l_2 s_{1+2} \end{bmatrix} , \quad \bB
    = \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} \end{align*} Note that this
    choice of the $\bC$ matrix was not unique. </div>

    <p> The manipulator equations are very general, but they do define some
    important characteristics.  For example, $\ddot{\bq}$ is (state-dependent)
    linearly related to the control input, $\bu$.   This observation justifies
    the control-affine form of the dynamics assumed throughout the notes.</p>

    <p>Note that we have chosen to use the notation of second-order systems
    (with $\dot{\bq}$ and $\ddot{\bq}$ appearing in the equations) throughout
    this book.  Although I believe it provides more clarity, there is an
    important limitation to this notation: it is impossible to describe 3D
    rotations in "minimal coordinates" using this notation without introducing
    kinematic singularities (like the famous "gimbal lock"). For instance, a
    common singularity-free choice for representing a 3D rotation is the unit
    quaternion, described by 4 real values (plus a norm constraint).  However we
    can still represent the rotational velocity without singularities using just
    3 real values.  This means that the length of our velocity vector is no
    longer the same as the length of our position vector.  For this reason, you
    will see that most of the software in <drake></drake> uses the more general
    notation with $\bv$ to represent velocity, $\bq$ to represent positions, and
    the manipulator equations are written as \begin{equation} \bM(\bq) \dot{\bv}
    + \bC(\bq,\bv)\bv = \btau_g(\bq) + \bB \bu, \end{equation} which is
    not necessarily a second-order system.  See <elib>Duindam06</elib> for a
    nice discussion of this topic.</p>

    <section data-type="sect2"><h1>Bilateral Position Constraints</h1>

      <p>If our robot has closed-kinematic chains, for instance those that arise
      from a <a href="https://en.wikipedia.org/wiki/Four-bar_linkage">four-bar
      linkage</a>, then we need a little more.  The Lagrangian machinery above
      assumes "minimal coordinates"; if our state vector $\bq$ contains all of
      the links in the kinematic chain, then we do not have a minimal
      parameterization -- each kinematic loop adds (at least) one constraint so
      should remove (at least) one degree of freedom.  Although some constraints
      can be solved away, the more general solution is to use the Lagrangian to
      derive the dynamics of the unconstrained system (a kinematic tree without
      the closed-loop constraints), then add additional generalized forces that
      ensure that the constraint is always satisfied. </p>

      <p>Consider the constraint equation \begin{equation}\bh(\bq) =
      0.\end{equation}  For the case of the kinematic closed-chain, this can be
      the kinematic constraint that the location of one end of the chain is
      equal to the location of the other end of the chain. The equations of
      motion can be written \begin{equation}\bM({\bq})\ddot{\bq} +
      \bC(\bq,\dot{\bq})\dot\bq = \btau_g(\bq) + \bB\bu + \bH^T(\bq)
      \blambda,\end{equation} where $\bH(\bq) = \pd\bh{\bq}$ and ${\blambda}$ is
      the constraint force.  Let's use the shorthand \begin{equation}
      \bM({\bq})\ddot{\bq} = \btau(q,\dot{q},u) + \bH^T(\bq) \blambda.
      \label{eq:manip_short} \end{equation}</p>

      <p>Using \begin{gather}\dot\bh = \bH\dot\bq,\\ \ddot\bh = \bH \ddot\bq +
      \dot\bH \dot\bq, \label{eq:ddoth} \end{gather} we can solve for
      $\blambda$, by observing that when the constraint is imposed, $\bh=0$ and
      therefore $\dot\bh=0$ and $\ddot\bh=0$.  Inserting the dynamics
      (\ref{eq:manip_short}) into (\ref{eq:ddoth}) yields
      \begin{equation}\blambda =- (\bH \bM^{-1} \bH^T)^+ (\bH \bM^{-1} \btau +
      \dot\bH\dot\bq).\end{equation} The $^+$ notation refers to a Moore-Penrose
      pseudo-inverse.  In many cases, this matrix will be full rank (for
      instance, in the case of multiple independent four-bar linkages) and the
      traditional inverse could have been used.  When the matrix drops rank
      (multiple solutions), then the pseudo-inverse will select the solution
      with the smallest constraint forces in the least-squares sense.</p>

      <p>To combat numerical "constraint drift", one might like to add a
      restoring force in the event that the constraint is not satisfied to
      numerical precision.  To accomplish this, rather than solving for
      $\ddot\bh = 0$ as above, we can instead solve for \begin{equation}\ddot\bh
      = \bH\ddot\bq + \dot\bH\dot\bq = -2\alpha \dot\bh - \alpha^2
      \bh,\end{equation} where $\alpha>0$ is a stiffness parameter. This is
      known as Baumgarte's stabilization technique, implemented here with a
      single parameter to provide a critically-damped response.
      Carrying
      this through yields \begin{equation} \blambda =- (\bH \bM^{-1} \bH^T)^+
      (\bH \bM^{-1} \btau + (\dot{\bH} + 2\alpha \bH)\dot{\bq} + \alpha^2 \bh).
      \end{equation}</p>

    </section>

    <section data-type="sect2"><h1>Bilateral Velocity Constraints</h1>

      <p>Consider the constraint equation \begin{equation}\bh_v(\bq,\dot{\bq}) =
      0,\end{equation} where $\pd{\bh_v}{\dot{\bq}} \ne 0.$  These are less
      common, but arise when, for instance, a joint is driven through a
      prescribed motion. Here, the manipulator equations are given by
      \begin{equation}\bM\ddot{\bq}  = \btau + \pd{\bh_v}{\dot{\bq}}^T
      \blambda.\end{equation} Using \begin{equation} \dot\bh_v = \pd{\bh_v}{\bq}
      \dot{\bq} + \pd{\bh_v}{\dot{\bq}}\ddot{\bq},\end{equation} setting
      $\dot\bh_v = 0$ yields \begin{equation}\blambda = - \left(
      \pd{\bh_v}{\dot{\bq}} \bM^{-1} \pd{\bh_v}{\dot{\bq}} \right)^+ \left[
      \pd{\bh_v}{\dot{\bq}} \bM^{-1} \btau + \pd{\bh_v}{\bq} \dot{\bq}
      \right].\end{equation}</p>

      <p>Again, to combat constraint drift, we can ask instead for $\dot\bh_v =
      -\alpha \bh_v$, which yields \begin{equation}\blambda = - \left(
      \pd{\bh_v}{\dot{\bq}} \bM^{-1} \pd{\bh_v}{\dot{\bq}} \right)^+ \left[
      \pd{\bh_v}{\dot{\bq}} \bM^{-1} \btau + \pd{\bh_v}{\bq} \dot{\bq} + \alpha
      \bh_v \right].\end{equation}</p>

    </section>

  </section>
  <section data-type="sect1"><h1>Contact Models</h1>

    <p>There are three main approaches used to modeling contact with "rigid"
    body systems:  1) rigid contact approximated with stiff compliant contact,
    2) rigid contact approximated with time-averaged forces (time-stepping), 3)
    hybrid models with collision event detection, impulsive reset maps, and
    continuous (constrained) dynamics between collision events.  Each modeling
    approach has advantages for different applications...</p>

    <todo>Finish this</todo>

    <section data-type="sect2"><h1>Impulsive Collisions</h1>

      <p>Now let $\phi(\bq)$ indicate the relative (signed) distance between two
      rigid bodies.  For rigid contact, we would like to enforce the unilateral
      constraint that \begin{equation} \phi(\bq) \geq 0. \end{equation} The
      collision event is described by the zero-crossings (from positive to
      negative) of $\phi$. Let us start by assuming frictionless collisions,
      allowing us to write \begin{equation}\bM\ddot{\bq} = \btau + \Phi^T
      \lambda,\end{equation} where $\Phi = \pd{\phi}{\bq}$ and $\lambda \ge 0$
      is now a (scalar) impulsive force that is well-defined when integrated
      over the time of the collision (denoted $t_c^-$ to $t_c^+$).  Integrate
      both sides of the equation over that (instantaneous) interval:
      \begin{align*}\int_{t_c^-}^{t_c^+} dt\left[  \bM \ddot{\bq} \right] =
      \int_{t_c^-}^{t_c^+} dt \left[ \btau + \Phi^T \lambda \right] \end{align*}
      Since $\bM$, $\btau$, and $\bPhi$ are constants over this interval, we are
      left with $$\bM\dot{\bq}^+ - \bM\dot{\bq}^- = \Phi^T \int_{t_c^-}^{t_c^+}
      \lambda dt,$$ where $\dot{\bq}^+$ is short-hand for $\dot{\bq}(t_c^+)$.
      Multiplying both sides by $\Phi \bM^{-1}$, we have $$ \Phi \dot{\bq}^+ -
      \Phi\dot{\bq}^- = \Phi \bM^{-1} \Phi^T \int_{t_c^-}^{t_c^+} \lambda dt.$$
      After the collision, we have $\dot\phi^+ = -e \dot\phi^-$, with $0 \le e
      \le 1$ denoting the <a
      href="https://en.wikipedia.org/wiki/Coefficient_of_restitution">coefficient
      of restitution</a>, yielding: $$ \Phi \dot{\bq}^+ - \Phi\dot{\bq}^- =
      -(1+e)\Phi\dot{\bq}^-,$$ and therefore $$\int_{t_c^-}^{t_c^+} \lambda dt =
      - (1+e)\left[ \Phi \bM^{-1} \Phi^T \right]^+ \Phi \dot{\bq}^-.$$
      Substituting this back in above results in \begin{equation}\dot{\bq}^+ =
      \left[ \bI - (1+e)\bM^{-1} \Phi^T \left[ \Phi \bM^{-1} \Phi^T \right]^+
      \Phi \right] \dot{\bq}^-.\end{equation}</p>

      <p>We can add friction at the contact.  Rigid bodies will almost always
      experience contact at only a point, so we typically ignore torsional
      friction <elib>Featherstone06</elib>, and model only tangential friction
      by replacing $\Phi$ with a matrix $\bJ$ that has $\Phi$ as one row, but
      two additional rows to capture the gradient of the contact location
      tangent to the contact surface, written in joint coordinates (c.f.
      <elib>Stewart96</elib>). Then $\blambda$ becomes a Cartesian force vector,
      but the equations above remain unchanged. If $\blambda$ is restricted to a
      friction cone, as in Coulomb friction, in general we have to solve an
      optimization to solve for $\dot\bq^+$ subject to the inequality
      constraints. </p>

    </section>

    <section data-type="sect2"><h1>Hybrid Dynamics</h1>

      <p> We can put the bilateral constraint equations and the impulsive
      collision equations together to implement a hybrid model for unilateral
      constraints of the form. Let us generalize \begin{equation}\bphi(\bq) \ge
      0,\end{equation} to be the vector of all pairwise (signed) distance
      functions between rigid bodies; this vector describes all possible
      contacts. At every moment, some subset of the contacts are active, and can
      be treated as a bilateral constraint ($\bphi_i=0$). The guards of the
      hybrid model are when an inactive constraint becomes active ($\bphi_i>0
      \rightarrow \bphi_i=0$), or when an active constraint becomes inactive
      ($\blambda_i>0 \rightarrow \blambda_i=0$ and $\dot\phi_i > 0$). Note that
      collision events will (almost always) only result in new active
      constraints when $e=0$, e.g. we have pure inelastic collisions, because
      elastic collisions will rarely permit sustained contact.</p>

      <p>If the contact involves Coulomb friction, then the transitions between
      sticking and sliding can be modeled as additional hybrid guards, or can be
      solved efficiently as a complementarity problem
      <elib>Stewart96</elib>.</p>

    </section>


    <section data-type="sect2"><h1>Compliant Contact Models</h1></section>


  </section>

  <section data-type="sect1"><h1>Recursive Dynamics Algorithms</h1>

    <p>The equations of motions for our machines get complicated quickly.
    Fortunately, for robots with a tree-link kinematic structure, there are very
    efficient and natural recursive algorithms for generating these equations of
    motion.  For a detailed reference on these methods, see
    <elib>Featherstone07</elib>; some people prefer reading about the
    Articulated Body Method in <elib>Mirtich96</elib>.  The implementation in
    <drake></drake> uses a related formulation from <elib>Jain11</elib>.</p>

  </section> <!-- end recursive algorithms -->

  <todo> numerical integration.  discrete-time approximations of continuous
  systems (lots of edx people had trouble with this.  some found:
  http://web.mit.edu/10.001/Web/Course_Notes/Differential_Equations_Notes/node3.html)
  </todo>

  <section data-type="sect1"><h1>Parameter Estimation</h1></section>

  <todo> lumped parameters.  identifiable lumped parameters.  least-squares
  estimation.  power formulations.  trajectory design.</todo>

</section> <!-- end rigid-body dynamics chapter -->


<section data-type="chapter" class="chapter"
         id="optimization"><h1>Optimization and
  Mathematical Programming</h1>

  <section data-type="sect1"><h1>Nonlinear optimization</h1>
    <section data-type="sect2"><h1>General formulation</h1>
    </section>
    <section data-type="sect2"><h1>Solution techniques</h1>
      Lagrange multipliers / KKT, Gradient descent, SQP (SNOPT, NLOPT, IPOPT), Global optimization
    </section>
    <section data-type="sect2"><h1>Example: Inverse Kinematics</h1>
    </section>
  </section>
  <section data-type="sect1"><h1>Convex optimization</h1>
    <section data-type="sect2"><h1>Linear Programs/Quadratic Programs/Second-Order Cones</h1>
      Example: Balancing force control on Atlas
    </section>
    <section data-type="sect2"><h1>Semidefinite Programming and Linear Matrix Inequalities</h1>
    </section>
    <section data-type="sect2"><h1>Sums of squares / polynomial optimization</h1>
    </section>
    <section data-type="sect2"><h1>Solution techniques</h1>
      Interior point (Gurobi, Mosek, Sedumi, ...), First order methods
    </section>
  </section>
  <section data-type="sect1"><h1>Mixed-integer convex optimization</h1></section>
  <section data-type="sect1"><h1>Combinatorial optimization</h1>
    Search, SAT, First order logic, SMT solvers, LP interpretation
  </section>

  <todo>Polynomial root finding/homotopy, L1 optimization, QCQP, LCP/Variational inequalities, BMI, ..., warm-starting, ...</todo>

</section> <!-- end optimization chapter -->

<section data-type="chapter" class="chapter" id="playbook"><h1>An Optimization
  Playbook</h1>

  <p>Coming soon... a collection of tips and formulations that can make
  seemingly non-smooth constraints smooth, seemingly non-convex constraints
  convex, etc.</p>

  <todo>PQ trick from LQR</todo>
  <todo>Hongkai's smooth non-penetration constraints for polygons</todo>

  <todo>"Soft" absolute value: $|x|_epsilon \approx \sqrt{x^2 +
    \epsilon}$.
    import numpy as np
    import matplotlib.pyplot as plt
    x = np.linspace(-2.,2.,101)
    plt.plot(x, np.sqrt(np.power(x,2) + 0.01))
  </todo>
</section> <!-- end playbook -->


</appendix>


<div id="footer">
<hr>
<table style="width:100%;">
  <tr><td><em>Underactuated Robotics</em></td><td align="right">&copy; Russ
    Tedrake, 2018</td></tr>
</table>
</div>


</body>
</html>
